From 8dac89ad43de22a9a4427e075199b3a0dd58774c Mon Sep 17 00:00:00 2001 From: Coreyboy1820 <92711317+Coreyboy1820@users.noreply.github.com> Date: Tue, 19 Dec 2023 21:34:49 -0800 Subject: [PATCH 1/6] :sparkles: Headerfile seperation (#101) * changed all file pathing to have seperated headers and implementations * fixing speed_sensor * cmake issues * still debugging mission_control * still debugging * fixed mission control linking issues * fixed variable names * :sparkles: seperated implementation from header files in drive which will allow us to match libhal and also create pre-built binaries to create an actual applicaiton out of our code --- drive/CMakeLists.txt | 8 ++ drive/applications/application.cpp | 10 +- drive/applications/application.hpp | 6 +- drive/implementations/command_lerper.cpp | 15 +++ drive/implementations/command_lerper.hpp | 22 ---- drive/implementations/mode_select.hpp | 14 +- drive/implementations/mode_switcher.cpp | 49 +++++++ drive/implementations/mode_switcher.hpp | 66 ---------- drive/implementations/rules_engine.hpp | 20 +-- drive/implementations/serial.hpp | 22 ---- drive/implementations/steer_modes.hpp | 96 +++++++------- ..._wheel_router.hpp => tri_wheel_router.cpp} | 22 +--- drive/include/command_lerper.hpp | 15 +++ .../drc_speed_sensor.hpp | 0 drive/include/esp8266_mission_control.hpp | 107 +++++++++++++++ .../mission_control.hpp | 2 +- drive/include/mode_switcher.hpp | 27 ++++ drive/include/offset_servo.hpp | 25 ++++ drive/include/serial_mission_control.hpp | 33 +++++ .../speed_sensor.hpp | 0 drive/include/tri_wheel_router.hpp | 33 +++++ .../drc_speed_sensor.cpp | 15 +-- .../esp8266_mission_control.cpp | 124 ++++-------------- drive/platform-implementations/home.hpp | 5 +- .../platform-implementations/offset_servo.cpp | 24 ++++ .../platform-implementations/offset_servo.hpp | 35 ----- .../print_mission_control.hpp | 41 ------ .../platform-implementations/print_motor.hpp | 28 ---- .../platform-implementations/print_servo.hpp | 28 ---- .../print_speed_sensor.hpp | 34 ----- .../serial_mission_control.cpp | 48 +++++++ drive/platforms/lpc4078.cpp | 31 ++--- 32 files changed, 512 insertions(+), 493 deletions(-) create mode 100644 drive/implementations/command_lerper.cpp delete mode 100644 drive/implementations/command_lerper.hpp create mode 100644 drive/implementations/mode_switcher.cpp delete mode 100644 drive/implementations/mode_switcher.hpp delete mode 100644 drive/implementations/serial.hpp rename drive/implementations/{tri_wheel_router.hpp => tri_wheel_router.cpp} (83%) create mode 100644 drive/include/command_lerper.hpp rename drive/{platform-implementations => include}/drc_speed_sensor.hpp (100%) create mode 100644 drive/include/esp8266_mission_control.hpp rename drive/{platform-implementations => include}/mission_control.hpp (97%) create mode 100644 drive/include/mode_switcher.hpp create mode 100644 drive/include/offset_servo.hpp create mode 100644 drive/include/serial_mission_control.hpp rename drive/{platform-implementations => include}/speed_sensor.hpp (100%) create mode 100644 drive/include/tri_wheel_router.hpp create mode 100644 drive/platform-implementations/offset_servo.cpp delete mode 100644 drive/platform-implementations/offset_servo.hpp delete mode 100644 drive/platform-implementations/print_mission_control.hpp delete mode 100644 drive/platform-implementations/print_motor.hpp delete mode 100644 drive/platform-implementations/print_servo.hpp delete mode 100644 drive/platform-implementations/print_speed_sensor.hpp create mode 100644 drive/platform-implementations/serial_mission_control.cpp diff --git a/drive/CMakeLists.txt b/drive/CMakeLists.txt index 8ffd77a..24dce68 100644 --- a/drive/CMakeLists.txt +++ b/drive/CMakeLists.txt @@ -5,6 +5,7 @@ project(drive VERSION 0.0.1 LANGUAGES CXX) set(platform_library $ENV{LIBHAL_PLATFORM_LIBRARY}) set(platform $ENV{LIBHAL_PLATFORM}) +set(CMAKE_VERBOSE_MAKEFILE ON) if("${platform_library}" STREQUAL "") message(FATAL_ERROR @@ -20,6 +21,13 @@ find_package(libhal-esp8266 REQUIRED CONFIG) add_executable(${PROJECT_NAME} main.cpp applications/application.cpp + platform-implementations/drc_speed_sensor.cpp + platform-implementations/esp8266_mission_control.cpp + platform-implementations/serial_mission_control.cpp + platform-implementations/offset_servo.cpp + implementations/command_lerper.cpp + implementations/mode_switcher.cpp + implementations/tri_wheel_router.cpp platforms/${platform}.cpp) target_include_directories(${PROJECT_NAME} PUBLIC .) target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_20) diff --git a/drive/applications/application.cpp b/drive/applications/application.cpp index 9995a55..61fd291 100644 --- a/drive/applications/application.cpp +++ b/drive/applications/application.cpp @@ -3,12 +3,12 @@ #include "../dto/motor_feedback.hpp" #include "../implementations/rules_engine.hpp" -#include "../implementations/command_lerper.hpp" +#include "../include/command_lerper.hpp" #include "../implementations/mode_select.hpp" -#include "../implementations/mode_switcher.hpp" -#include "../implementations/tri_wheel_router.hpp" +#include "../include/mode_switcher.hpp" +#include "../include/tri_wheel_router.hpp" -#include "../platform-implementations/mission_control.hpp" +#include "../include/mission_control.hpp" #include "application.hpp" namespace sjsu::drive { @@ -49,7 +49,7 @@ hal::status application(application_framework& p_framework) commands = sjsu::drive::validate_commands(commands); commands = mode_switcher.switch_steer_mode( - commands, arguments, motor_speeds, terminal); + commands, arguments, motor_speeds); commands.speed = lerp.lerp(commands.speed); arguments = sjsu::drive::select_mode(commands); diff --git a/drive/applications/application.hpp b/drive/applications/application.hpp index e4bbd39..fb96671 100644 --- a/drive/applications/application.hpp +++ b/drive/applications/application.hpp @@ -6,9 +6,9 @@ #include #include #include -#include "../platform-implementations/speed_sensor.hpp" -#include "../platform-implementations/mission_control.hpp" -#include "../platform-implementations/offset_servo.hpp" +#include "../include/speed_sensor.hpp" +#include "../include/mission_control.hpp" +#include "../include/offset_servo.hpp" namespace sjsu::drive { diff --git a/drive/implementations/command_lerper.cpp b/drive/implementations/command_lerper.cpp new file mode 100644 index 0000000..ccc09ea --- /dev/null +++ b/drive/implementations/command_lerper.cpp @@ -0,0 +1,15 @@ + +#include "../dto/drive.hpp" +#include "../include/command_lerper.hpp" + +namespace sjsu::drive { + + int command_lerper::lerp(float p_speed) + { + m_previous_speed = + static_cast(std::lerp(m_previous_speed, + p_speed, + m_lerp_speed)); + return m_previous_speed; + } +} // namespace drive diff --git a/drive/implementations/command_lerper.hpp b/drive/implementations/command_lerper.hpp deleted file mode 100644 index 653233c..0000000 --- a/drive/implementations/command_lerper.hpp +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once - -#include "../dto/drive.hpp" - -namespace sjsu::drive { -class command_lerper -{ -public: - int lerp(int speed) - { - previous_speed = - static_cast(std::lerp(static_cast(previous_speed), - static_cast(speed), - speed_lerp)); - return previous_speed; - } - -private: - static constexpr float speed_lerp = 0.1f; - float previous_speed = 0; // don't touch mode or wheel orientation logic -}; -} // namespace Drive \ No newline at end of file diff --git a/drive/implementations/mode_select.hpp b/drive/implementations/mode_select.hpp index 3b51d84..7a05901 100644 --- a/drive/implementations/mode_select.hpp +++ b/drive/implementations/mode_select.hpp @@ -1,22 +1,22 @@ #pragma once #include "../dto/drive.hpp" -#include "mode_switcher.hpp" +#include "../include/mode_switcher.hpp" #include "steer_modes.hpp" -#include "../platform-implementations/mission_control.hpp" +#include "../include/mission_control.hpp" namespace sjsu::drive { -inline tri_wheel_router_arguments select_mode(mission_control::mc_commands commands) +inline tri_wheel_router_arguments select_mode(mission_control::mc_commands p_commands) { - switch (commands.mode) { + switch (p_commands.mode) { case 'D': - return drive_steering(commands); + return drive_steering(p_commands); break; case 'S': - return spin_steering(commands); + return spin_steering(p_commands); break; case 'T': - return translate_steering(commands); + return translate_steering(p_commands); break; case 'H': // do nothing to get it into drive mode diff --git a/drive/implementations/mode_switcher.cpp b/drive/implementations/mode_switcher.cpp new file mode 100644 index 0000000..cf06781 --- /dev/null +++ b/drive/implementations/mode_switcher.cpp @@ -0,0 +1,49 @@ +#include "../dto/drive.hpp" +#include "../dto/motor_feedback.hpp" +#include "../include/mission_control.hpp" +#include "../include/mode_switcher.hpp" + +namespace sjsu::drive { + mission_control::mc_commands mode_switch::switch_steer_mode( + mission_control::mc_commands p_commands, + tri_wheel_router_arguments p_previous_arguments, + motor_feedback p_current_motor_feedback) + { + bool hubs_stopped((p_previous_arguments.left.speed >= -0.01f && + p_previous_arguments.left.speed <= 0.01f) && + (p_previous_arguments.right.speed >= -0.01f && + p_previous_arguments.right.speed <= 0.01f) && + (p_previous_arguments.back.speed >= -0.01f && + p_previous_arguments.back.speed <= 0.01f)); + + bool steers_stopped((p_current_motor_feedback.left_steer_speed >= -0.01f && + p_current_motor_feedback.left_steer_speed <= 0.01f) && + (p_current_motor_feedback.right_steer_speed >= -0.01f && + p_current_motor_feedback.right_steer_speed <= 0.01f) && + (p_current_motor_feedback.back_steer_speed >= -0.01f && + p_current_motor_feedback.back_steer_speed <= 0.01f)); + + if (m_previous_mode != p_commands.mode) { + m_in_the_middle_of_switching_modes = true; + m_skip_once = true; + } + + if (m_in_the_middle_of_switching_modes) { + p_commands.speed = 0; + if (!hubs_stopped) { + p_commands.mode = m_previous_mode; + return p_commands; + } // hubs must be stopped to pass here + + else if (!m_skip_once && steers_stopped) { + m_in_the_middle_of_switching_modes = false; + m_skip_once = true; + } // only once steer motors have stopped moving after hubs stopped will + // we exit switching modes + m_previous_mode = p_commands.mode; + m_skip_once = false; + } + + return p_commands; + } +} \ No newline at end of file diff --git a/drive/implementations/mode_switcher.hpp b/drive/implementations/mode_switcher.hpp deleted file mode 100644 index b2d0376..0000000 --- a/drive/implementations/mode_switcher.hpp +++ /dev/null @@ -1,66 +0,0 @@ -#pragma once - -#include "../dto/drive.hpp" -#include "../dto/motor_feedback.hpp" -#include "../platform-implementations/mission_control.hpp" - -namespace sjsu::drive { -class mode_switch -{ -public: - // Should handle all the logic for switching to new steer mode - mission_control::mc_commands switch_steer_mode( - mission_control::mc_commands commands, - tri_wheel_router_arguments previous_arguments, - motor_feedback current_motor_feedback, - hal::serial& terminal) - { - bool hubs_stopped((previous_arguments.left.speed >= -0.01f && - previous_arguments.left.speed <= 0.01f) && - (previous_arguments.right.speed >= -0.01f && - previous_arguments.right.speed <= 0.01f) && - (previous_arguments.back.speed >= -0.01f && - previous_arguments.back.speed <= 0.01f)); - - bool steers_stopped((current_motor_feedback.left_steer_speed >= -0.01f && - current_motor_feedback.left_steer_speed <= 0.01f) && - (current_motor_feedback.right_steer_speed >= -0.01f && - current_motor_feedback.right_steer_speed <= 0.01f) && - (current_motor_feedback.back_steer_speed >= -0.01f && - current_motor_feedback.back_steer_speed <= 0.01f)); - - if (previous_mode_ != commands.mode) { - in_the_middle_of_switching_modes_ = true; - skip_once_ = true; - } - - if (in_the_middle_of_switching_modes_) { - commands.speed = 0; - if (!hubs_stopped) { - commands.mode = previous_mode_; - return commands; - } // hubs must be stopped to pass here - - else if (!skip_once_ && steers_stopped) { - in_the_middle_of_switching_modes_ = false; - skip_once_ = true; - } // only once steer motors have stopped moving after hubs stopped will - // we exit switching modes - previous_mode_ = commands.mode; - skip_once_ = false; - } - - return commands; - } - -private: - - char previous_mode_ = - 'H'; // This is H for homing when the rover turns on, it makes sure that we - // don't switch to drive mode and allow commands to be parsed through - bool in_the_middle_of_switching_modes_ = true; - bool skip_once_ = true; - -}; - -} // namespace Drive diff --git a/drive/implementations/rules_engine.hpp b/drive/implementations/rules_engine.hpp index 64ff76e..3e25862 100644 --- a/drive/implementations/rules_engine.hpp +++ b/drive/implementations/rules_engine.hpp @@ -1,25 +1,25 @@ #pragma once -#include "../platform-implementations/mission_control.hpp" +#include "../include/mission_control.hpp" namespace sjsu::drive { -inline mission_control::mc_commands validate_commands(mission_control::mc_commands commands) +inline mission_control::mc_commands validate_commands(mission_control::mc_commands p_commands) { static constexpr int kMaxSpeed = 100; static constexpr int kMaxAngle = 12; - if (commands.mode == 'D') { - commands.angle = std::clamp(commands.angle, -kMaxAngle, kMaxAngle); + if (p_commands.mode == 'D') { + p_commands.angle = std::clamp(p_commands.angle, -kMaxAngle, kMaxAngle); } // TODO: implement heartbeat logic - if (!commands.is_operational) { - commands.speed = 0; - return commands; + if (!p_commands.is_operational) { + p_commands.speed = 0; + return p_commands; } - if (commands.speed > kMaxSpeed || commands.speed < -kMaxSpeed) { - commands.speed = std::clamp(commands.speed, -kMaxSpeed, kMaxSpeed); + if (p_commands.speed > kMaxSpeed || p_commands.speed < -kMaxSpeed) { + p_commands.speed = std::clamp(p_commands.speed, -kMaxSpeed, kMaxSpeed); } - return commands; + return p_commands; } } // namespace Drive \ No newline at end of file diff --git a/drive/implementations/serial.hpp b/drive/implementations/serial.hpp deleted file mode 100644 index f4c49ca..0000000 --- a/drive/implementations/serial.hpp +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once - -#include - -#include - -namespace Drive { -inline hal::result get_commands(hal::serial& terminal) -{ - std::array buffer{}; - auto received = HAL_CHECK(terminal.read(buffer)).data; - if (received.size() != 0) { - auto response = std::string_view( - reinterpret_cast(received.data()), received.size()); - hal::print<200>( - terminal, "%.*s", static_cast(response.size()), response.data()); - return response; - } else { - return std::string_view(""); - } -} -} // namespace Drive \ No newline at end of file diff --git a/drive/implementations/steer_modes.hpp b/drive/implementations/steer_modes.hpp index 6418dbf..a51b07f 100644 --- a/drive/implementations/steer_modes.hpp +++ b/drive/implementations/steer_modes.hpp @@ -2,7 +2,7 @@ #include #include -#include "../platform-implementations/mission_control.hpp" +#include "../include/mission_control.hpp" #include "../dto/drive.hpp" namespace sjsu::drive { @@ -16,97 +16,97 @@ static constexpr float right_leg_spin_offset = 0; static constexpr float back_leg_spin_offset = 0; -inline tri_wheel_router_arguments spin_steering(mission_control::mc_commands commands) +inline tri_wheel_router_arguments spin_steering(mission_control::mc_commands p_commands) { tri_wheel_router_arguments temp; temp.back.angle = back_leg_spin_offset; temp.left.angle = left_leg_spin_offset; temp.right.angle = right_leg_spin_offset; - temp.back.speed = static_cast(-commands.speed); - temp.left.speed = static_cast(commands.speed); - temp.right.speed = static_cast(-commands.speed); + temp.back.speed = static_cast(-p_commands.speed); + temp.left.speed = static_cast(p_commands.speed); + temp.right.speed = static_cast(-p_commands.speed); return temp; } -inline tri_wheel_router_arguments translate_steering(mission_control::mc_commands commands) +inline tri_wheel_router_arguments translate_steering(mission_control::mc_commands p_commands) { tri_wheel_router_arguments steer_arguments; steer_arguments.left.angle = - (static_cast(-commands.angle) + left_leg_drive_offset); + (static_cast(-p_commands.angle) + left_leg_drive_offset); steer_arguments.right.angle = - (static_cast(-commands.angle) + right_leg_drive_offset); + (static_cast(-p_commands.angle) + right_leg_drive_offset); steer_arguments.back.angle = - (static_cast(-commands.angle) + back_leg_drive_offset); + (static_cast(-p_commands.angle) + back_leg_drive_offset); - steer_arguments.left.speed = static_cast(-commands.speed); - steer_arguments.right.speed = static_cast(-commands.speed); - steer_arguments.back.speed = static_cast(commands.speed); + steer_arguments.left.speed = static_cast(-p_commands.speed); + steer_arguments.right.speed = static_cast(-p_commands.speed); + steer_arguments.back.speed = static_cast(p_commands.speed); return steer_arguments; } /// Ackerman steering equation to compute outter wheel angle -inline float calculate_ackerman(float outter_wheel_angle) +inline float calculate_ackerman(float p_outter_wheel_angle) { - float inner_wheel_angle = float(-.583 + 1.97 * abs(int(outter_wheel_angle)) + - -.224 * pow(abs(int(outter_wheel_angle)), 2) + - .0278 * pow(abs(int(outter_wheel_angle)), 3)); - return (outter_wheel_angle > 0) ? inner_wheel_angle : -inner_wheel_angle; + float inner_wheel_angle = float(-.583 + 1.97 * abs(int(p_outter_wheel_angle)) + + -.224 * pow(abs(int(p_outter_wheel_angle)), 2) + + .0278 * pow(abs(int(p_outter_wheel_angle)), 3)); + return (p_outter_wheel_angle > 0) ? inner_wheel_angle : -inner_wheel_angle; } -inline float get_inner_wheel_radius(float outter_wheel_angle) +inline float get_inner_wheel_radius(float p_outter_wheel_angle) { - return (15.0f * std::pow(std::abs(outter_wheel_angle), -.971f)); + return (15.0f * std::pow(std::abs(p_outter_wheel_angle), -.971f)); } -inline float get_back_wheel_radius(float outter_wheel_angle) +inline float get_back_wheel_radius(float p_outter_wheel_angle) { - return (11.6f * std::pow(std::abs(outter_wheel_angle), -.698f)); + return (11.6f * std::pow(std::abs(p_outter_wheel_angle), -.698f)); } -inline float get_outter_wheel_radius(float outter_wheel_angle) +inline float get_outter_wheel_radius(float p_outter_wheel_angle) { - return (11.6f * std::pow(std::abs(outter_wheel_angle), -.616f)); + return (11.6f * std::pow(std::abs(p_outter_wheel_angle), -.616f)); } -inline float get_inner_wheel_hub_speed(float outter_wheel_speed, - float outter_wheel_angle) +inline float get_inner_wheel_hub_speed(float p_outter_wheel_speed, + float p_outter_wheel_angle) { // clamps the inner wheel speed to be no faster then what will mess up the // correct ackerman velocity this clamp will then ensure the same for the // back wheel speed since its based on this angle - float ratio = get_inner_wheel_radius(outter_wheel_angle) / - get_outter_wheel_radius(outter_wheel_angle); + float ratio = get_inner_wheel_radius(p_outter_wheel_angle) / + get_outter_wheel_radius(p_outter_wheel_angle); // std::clamp(inner_wheel_speed, -100 / ratio, 100 / ratio); - return (outter_wheel_speed * ratio); + return (p_outter_wheel_speed * ratio); } -inline float get_back_wheel_hub_speed(float outter_wheel_speed, - float outter_wheel_angle) +inline float get_back_wheel_hub_speed(float p_outter_wheel_speed, + float p_outter_wheel_angle) { - float ratio = get_back_wheel_radius(outter_wheel_angle) / - get_outter_wheel_radius(outter_wheel_angle); + float ratio = get_back_wheel_radius(p_outter_wheel_angle) / + get_outter_wheel_radius(p_outter_wheel_angle); // std::clamp(inner_wheel_speed * ratio, -100 / ratio, 100 / ratio); - return (outter_wheel_speed * ratio); + return (p_outter_wheel_speed * ratio); } -inline tri_wheel_router_arguments drive_steering(mission_control::mc_commands commands) +inline tri_wheel_router_arguments drive_steering(mission_control::mc_commands p_commands) { float outter_wheel_angle = 0.0, back_wheel_angle = 0.0; tri_wheel_router_arguments steer_arguments; - if (commands.angle > 0) { - outter_wheel_angle = static_cast(-commands.angle); + if (p_commands.angle > 0) { + outter_wheel_angle = static_cast(-p_commands.angle); steer_arguments.left.angle = outter_wheel_angle; steer_arguments.right.angle = calculate_ackerman(outter_wheel_angle); - } else if (commands.angle < 0) { - outter_wheel_angle = static_cast(-commands.angle); + } else if (p_commands.angle < 0) { + outter_wheel_angle = static_cast(-p_commands.angle); steer_arguments.right.angle = outter_wheel_angle; steer_arguments.left.angle = calculate_ackerman(outter_wheel_angle); } - if (commands.angle == 0) { + if (p_commands.angle == 0) { steer_arguments.back.angle = 0; steer_arguments.left.angle = 0; steer_arguments.right.angle = 0; @@ -122,22 +122,22 @@ inline tri_wheel_router_arguments drive_steering(mission_control::mc_commands co if (outter_wheel_angle > 0) { steer_arguments.right.speed = -get_inner_wheel_hub_speed( - static_cast(commands.speed), outter_wheel_angle); - steer_arguments.left.speed = -static_cast(commands.speed); + static_cast(p_commands.speed), outter_wheel_angle); + steer_arguments.left.speed = -static_cast(p_commands.speed); steer_arguments.back.speed = get_back_wheel_hub_speed( - static_cast(commands.speed), outter_wheel_angle); + static_cast(p_commands.speed), outter_wheel_angle); } else if (outter_wheel_angle < 0) { steer_arguments.left.speed = -get_inner_wheel_hub_speed( - static_cast(commands.speed), outter_wheel_angle); - steer_arguments.right.speed = -static_cast(commands.speed); + static_cast(p_commands.speed), outter_wheel_angle); + steer_arguments.right.speed = -static_cast(p_commands.speed); steer_arguments.back.speed = get_back_wheel_hub_speed( - static_cast(commands.speed), outter_wheel_angle); + static_cast(p_commands.speed), outter_wheel_angle); } else { - steer_arguments.left.speed = static_cast(-commands.speed); - steer_arguments.right.speed = static_cast(-commands.speed); - steer_arguments.back.speed = static_cast(commands.speed); + steer_arguments.left.speed = static_cast(-p_commands.speed); + steer_arguments.right.speed = static_cast(-p_commands.speed); + steer_arguments.back.speed = static_cast(p_commands.speed); } // adding the offsets to put it into the base drive mode diff --git a/drive/implementations/tri_wheel_router.hpp b/drive/implementations/tri_wheel_router.cpp similarity index 83% rename from drive/implementations/tri_wheel_router.hpp rename to drive/implementations/tri_wheel_router.cpp index 5250a10..b7e4855 100644 --- a/drive/implementations/tri_wheel_router.hpp +++ b/drive/implementations/tri_wheel_router.cpp @@ -1,5 +1,3 @@ -#pragma once - #include #include #include @@ -9,20 +7,18 @@ #include "../dto/drive.hpp" #include "../dto/motor_feedback.hpp" #include "../applications/application.hpp" +#include "../include/tri_wheel_router.hpp" namespace sjsu::drive { -class tri_wheel_router -{ -public: - tri_wheel_router(leg& p_back, leg& p_right, leg& p_left) + tri_wheel_router::tri_wheel_router(leg& p_back, leg& p_right, leg& p_left) : m_left(p_left) , m_back(p_back) , m_right(p_right) { } - hal::status move(tri_wheel_router_arguments p_tri_wheel_arguments, + hal::status tri_wheel_router::move(tri_wheel_router_arguments p_tri_wheel_arguments, hal::steady_clock& p_clock) { HAL_CHECK(m_left.steer->position(-p_tri_wheel_arguments.left.angle)); @@ -37,7 +33,7 @@ class tri_wheel_router return hal::success(); } - hal::result get_motor_feedback() + hal::result tri_wheel_router::get_motor_feedback() { using namespace std::chrono_literals; using namespace hal::literals; @@ -53,12 +49,4 @@ class tri_wheel_router return motor_speeds; } -private: - - // member variables - - leg& m_left; - leg& m_back; - leg& m_right; -}; -} // namespace Drive +} \ No newline at end of file diff --git a/drive/include/command_lerper.hpp b/drive/include/command_lerper.hpp new file mode 100644 index 0000000..9c083e4 --- /dev/null +++ b/drive/include/command_lerper.hpp @@ -0,0 +1,15 @@ +#pragma once + +#include "../dto/drive.hpp" + +namespace sjsu::drive { +class command_lerper +{ +public: + int lerp(float p_speed); + +private: + static constexpr float m_lerp_speed = 0.1f; + float m_previous_speed = 0; +}; +} // namespace drive diff --git a/drive/platform-implementations/drc_speed_sensor.hpp b/drive/include/drc_speed_sensor.hpp similarity index 100% rename from drive/platform-implementations/drc_speed_sensor.hpp rename to drive/include/drc_speed_sensor.hpp diff --git a/drive/include/esp8266_mission_control.hpp b/drive/include/esp8266_mission_control.hpp new file mode 100644 index 0000000..02624c9 --- /dev/null +++ b/drive/include/esp8266_mission_control.hpp @@ -0,0 +1,107 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include "mission_control.hpp" + +namespace sjsu::drive{ + +class esp8266_mission_control : public mission_control +{ +public: + + struct create_t { + hal::esp8266::at& esp8266; + hal::serial& console; + const std::string_view ssid; + const std::string_view password; + const hal::esp8266::at::socket_config& config; + const std::string_view ip; + std::span buffer; + std::string_view get_request; + }; + + struct http_header_parser_t + { + hal::stream_find find_header_start; + hal::stream_find find_content_length; + hal::stream_parse parse_content_length; + hal::stream_find find_end_of_header; + }; + + enum class connection_state + { + check_ap_connection, + connecting_to_ap, + set_ip_address, + check_server_connection, + connecting_to_server, + connection_established, + }; + + [[nodiscard]] static hal::result create(create_t p_create, hal::timeout auto& p_timeout) + { + esp8266_mission_control esp_mission_control(p_create.esp8266, + p_create.console, + p_create.ssid, + p_create.password, + p_create.config, + p_create.ip, + p_create.buffer, + p_create.get_request); + HAL_CHECK(esp_mission_control.establish_connection(p_timeout)); + + return esp_mission_control; + } + + +private: + + esp8266_mission_control(hal::esp8266::at& p_esp8266, + hal::serial& p_console, + const std::string_view p_ssid, + const std::string_view p_password, + const hal::esp8266::at::socket_config& p_config, + const std::string_view p_ip, + std::span p_buffer, + std::string_view p_get_request); + + hal::result impl_get_command(hal::function_ref p_timeout) override; + + void parse_commands(); + + std::string_view to_string_view(std::span p_span); + + hal::status establish_connection(hal::function_ref p_timeout); + + http_header_parser_t new_http_header_parser(); + + mc_commands m_commands{}; + http_header_parser_t m_http_header_parser; + hal::stream_fill m_fill_payload; + const hal::esp8266::at::socket_config& m_config; + hal::esp8266::at* m_esp8266; + hal::serial* m_console; + std::string_view m_ssid; + std::string_view m_password; + std::string_view m_get_request; + std::string_view m_ip; + std::span m_buffer; + std::array m_command_buffer; + size_t m_buffer_len; + size_t m_content_length; + bool m_write_error = false; + bool m_header_finished = false; + bool m_read_complete = true; + std::uint32_t m_missed_read = 0; +}; + +} diff --git a/drive/platform-implementations/mission_control.hpp b/drive/include/mission_control.hpp similarity index 97% rename from drive/platform-implementations/mission_control.hpp rename to drive/include/mission_control.hpp index 0ba3802..ac02b8e 100644 --- a/drive/platform-implementations/mission_control.hpp +++ b/drive/include/mission_control.hpp @@ -76,7 +76,7 @@ class mission_control virtual ~mission_control() = default; private: - virtual hal::result impl_get_command( + virtual hal::result impl_get_command( hal::function_ref p_timeout) = 0; }; diff --git a/drive/include/mode_switcher.hpp b/drive/include/mode_switcher.hpp new file mode 100644 index 0000000..b386f02 --- /dev/null +++ b/drive/include/mode_switcher.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include "../dto/drive.hpp" +#include "../dto/motor_feedback.hpp" +#include "../include/mission_control.hpp" + +namespace sjsu::drive { +class mode_switch +{ +public: + // Should handle all the logic for switching to new steer mode + mission_control::mc_commands switch_steer_mode( + mission_control::mc_commands p_commands, + tri_wheel_router_arguments p_previous_arguments, + motor_feedback p_current_motor_feedback); + +private: + + char m_previous_mode = + 'H'; // This is H for homing when the rover turns on, it makes sure that we + // don't switch to drive mode and allow commands to be parsed through + bool m_in_the_middle_of_switching_modes = true; + bool m_skip_once = true; + +}; + +} // namespace Drive diff --git a/drive/include/offset_servo.hpp b/drive/include/offset_servo.hpp new file mode 100644 index 0000000..9db0882 --- /dev/null +++ b/drive/include/offset_servo.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +namespace sjsu::drive { + +class offset_servo : public hal::servo { +public: + static hal::result create(hal::servo& p_servo, hal::degrees p_offset); + + hal::degrees get_offset(); + + void set_offset(hal::degrees p_offset); + +private: + offset_servo(hal::servo& p_servo, hal::degrees p_offset); + + hal::result driver_position(hal::degrees p_position) override; + + hal::degrees m_offset; + hal::servo* m_servo = nullptr; +}; + +} diff --git a/drive/include/serial_mission_control.hpp b/drive/include/serial_mission_control.hpp new file mode 100644 index 0000000..f0aa5ac --- /dev/null +++ b/drive/include/serial_mission_control.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include + +#include + +#include "mission_control.hpp" + +namespace sjsu::drive { + +class serial_mission_control : public mission_control{ +public: + +[[nodiscard]] static hal::result create(hal::serial& p_console) + { + serial_mission_control s_mission_control(p_console); + + return s_mission_control; + } + +private: + + serial_mission_control(hal::serial& p_console); + + void parse_commands(std::string_view p_commands_json); + + hal::result impl_get_command( + hal::function_ref p_timeout) override; + + hal::serial& m_console; + mc_commands m_commands; +}; +} // namespace Drive \ No newline at end of file diff --git a/drive/platform-implementations/speed_sensor.hpp b/drive/include/speed_sensor.hpp similarity index 100% rename from drive/platform-implementations/speed_sensor.hpp rename to drive/include/speed_sensor.hpp diff --git a/drive/include/tri_wheel_router.hpp b/drive/include/tri_wheel_router.hpp new file mode 100644 index 0000000..1b1b45b --- /dev/null +++ b/drive/include/tri_wheel_router.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "../dto/drive.hpp" +#include "../dto/motor_feedback.hpp" +#include "../applications/application.hpp" + +namespace sjsu::drive { +class tri_wheel_router +{ +public: + + tri_wheel_router(leg& p_back, leg& p_right, leg& p_left); + + hal::status move(tri_wheel_router_arguments p_tri_wheel_arguments, + hal::steady_clock& p_clock); + + hal::result get_motor_feedback(); + +private: + + // member variables + + leg& m_left; + leg& m_back; + leg& m_right; +}; +} // namespace Drive diff --git a/drive/platform-implementations/drc_speed_sensor.cpp b/drive/platform-implementations/drc_speed_sensor.cpp index 2aaa45a..3545e8b 100644 --- a/drive/platform-implementations/drc_speed_sensor.cpp +++ b/drive/platform-implementations/drc_speed_sensor.cpp @@ -1,15 +1,8 @@ -#pragma once - -#include "speed_sensor.hpp" -#include "drc_speed_sensor.hpp" +#include "../include/drc_speed_sensor.hpp" #include namespace sjsu::drive { - hal::result make_speed_sensor(hal::rmd::drc& p_drc) { - return drc_speed_sensor(p_drc); - } - drc_speed_sensor::drc_speed_sensor(hal::rmd::drc& p_drc) : m_drc(&p_drc) { @@ -19,5 +12,9 @@ namespace sjsu::drive { HAL_CHECK(m_drc->feedback_request(hal::rmd::drc::read::status_2)); return speed_sensor::read_t{.speed = m_drc->feedback().speed()}; - } + } + + hal::result make_speed_sensor(hal::rmd::drc& p_drc) { + return drc_speed_sensor(p_drc); + } } \ No newline at end of file diff --git a/drive/platform-implementations/esp8266_mission_control.cpp b/drive/platform-implementations/esp8266_mission_control.cpp index fccf046..1e2f19a 100644 --- a/drive/platform-implementations/esp8266_mission_control.cpp +++ b/drive/platform-implementations/esp8266_mission_control.cpp @@ -1,6 +1,3 @@ -#pragma once - -#include "mission_control.hpp" #include #include #include @@ -12,38 +9,13 @@ #include #include -namespace sjsu::drive { +#include "../include/mission_control.hpp" +#include "../include/esp8266_mission_control.hpp" -class esp8266_mission_control : public mission_control -{ -public: - static hal::result create( - hal::esp8266::at& p_esp8266, - hal::serial& p_console, - const std::string_view p_ssid, - const std::string_view p_password, - const hal::esp8266::at::socket_config& p_config, - const std::string_view p_ip, - hal::timeout auto& p_timeout, - std::span p_buffer, - std::string_view p_get_request) - { - esp8266_mission_control esp_mission_control = - esp8266_mission_control(p_esp8266, - p_console, - p_ssid, - p_password, - p_config, - p_ip, - p_buffer, - p_get_request); - HAL_CHECK(esp_mission_control.establish_connection(p_timeout)); +namespace sjsu::drive { - return esp_mission_control; - } -private: - esp8266_mission_control(hal::esp8266::at& p_esp8266, + esp8266_mission_control::esp8266_mission_control(hal::esp8266::at& p_esp8266, hal::serial& p_console, const std::string_view p_ssid, const std::string_view p_password, @@ -61,12 +33,12 @@ class esp8266_mission_control : public mission_control , m_get_request(p_get_request) , m_fill_payload(hal::stream_fill(m_buffer)) , m_http_header_parser(new_http_header_parser()) + , m_buffer_len(0) { - m_buffer_len = 0; } - hal::result impl_get_command( - hal::function_ref p_timeout) override + hal::result esp8266_mission_control::impl_get_command( + hal::function_ref p_timeout) { using namespace std::literals; @@ -74,7 +46,6 @@ class esp8266_mission_control : public mission_control if (m_write_error) { hal::print(*m_console, "Reconnecting...\n"); // Wait 1s before attempting to reconnect - auto result = establish_connection(p_timeout); if (!result) { hal::print(*m_console, "Failure!!!\n"); @@ -159,7 +130,7 @@ class esp8266_mission_control : public mission_control return m_commands; } - void parse_commands() { + void esp8266_mission_control::parse_commands() { auto result = to_string_view(m_command_buffer); static constexpr int expected_number_of_arguments = 6; @@ -179,36 +150,31 @@ class esp8266_mission_control : public mission_control actual_arguments, expected_number_of_arguments); } - // hal::print<200>(*m_console, - // "HB: %d\t, IO %d\t, WO: %d\t, DM: %c\t, Speed: %d\n, Angle: %d\n", - // commands.heartbeat_count, - // commands.is_operational, - // commands.wheel_orientation, - // commands.mode, - // commands.speed, - // commands.angle - // ); + m_commands = commands; } + esp8266_mission_control::http_header_parser_t esp8266_mission_control::new_http_header_parser() + { + using namespace std::literals; + + return esp8266_mission_control::http_header_parser_t{ + .find_header_start = hal::stream_find(hal::as_bytes("HTTP/1.1 "sv)), + .find_content_length = + hal::stream_find(hal::as_bytes("Content-Length: "sv)), + .parse_content_length = hal::stream_parse(), + .find_end_of_header = hal::stream_find(hal::as_bytes("\r\n\r\n"sv)), + }; + } - std::string_view to_string_view(std::span p_span) + + std::string_view esp8266_mission_control::to_string_view(std::span p_span) { return std::string_view(reinterpret_cast(p_span.data()), p_span.size()); } - enum class connection_state - { - check_ap_connection, - connecting_to_ap, - set_ip_address, - check_server_connection, - connecting_to_server, - connection_established, - }; - - [[nodiscard]] hal::status establish_connection(hal::timeout auto& p_timeout) + hal::status esp8266_mission_control::establish_connection(hal::function_ref p_timeout) { connection_state state = connection_state::check_ap_connection; @@ -274,44 +240,4 @@ class esp8266_mission_control : public mission_control return hal::success(); } - struct http_header_parser_t - { - hal::stream_find find_header_start; - hal::stream_find find_content_length; - hal::stream_parse parse_content_length; - hal::stream_find find_end_of_header; - }; - - http_header_parser_t new_http_header_parser() - { - using namespace std::literals; - - return http_header_parser_t{ - .find_header_start = hal::stream_find(hal::as_bytes("HTTP/1.1 "sv)), - .find_content_length = - hal::stream_find(hal::as_bytes("Content-Length: "sv)), - .parse_content_length = hal::stream_parse(), - .find_end_of_header = hal::stream_find(hal::as_bytes("\r\n\r\n"sv)), - }; - } - - mc_commands m_commands{}; - hal::esp8266::at* m_esp8266; - hal::serial* m_console; - std::string_view m_ssid; - std::string_view m_password; - const hal::esp8266::at::socket_config& m_config; - std::string_view m_ip; - std::span m_buffer; - std::array m_command_buffer; - std::string_view m_get_request; - http_header_parser_t m_http_header_parser; - size_t m_buffer_len; - bool m_write_error = false; - bool m_header_finished = false; - bool m_read_complete = true; - hal::stream_fill m_fill_payload; - size_t m_content_length; - std::uint32_t m_missed_read = 0; -}; -} // namespace sjsu::arm \ No newline at end of file +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/platform-implementations/home.hpp b/drive/platform-implementations/home.hpp index 2f77cfc..88113a2 100644 --- a/drive/platform-implementations/home.hpp +++ b/drive/platform-implementations/home.hpp @@ -5,7 +5,7 @@ #include #include #include -#include "offset_servo.hpp" +#include "../include/offset_servo.hpp" namespace sjsu::drive { @@ -17,8 +17,7 @@ struct homing { // this function will return offsets in the same order of servos passed in inline hal::status home(std::span p_homing_structs, - hal::steady_clock& p_counter, - hal::serial* terminal) { + hal::steady_clock& p_counter) { using namespace std::chrono_literals; using namespace hal::literals; diff --git a/drive/platform-implementations/offset_servo.cpp b/drive/platform-implementations/offset_servo.cpp new file mode 100644 index 0000000..7f5b047 --- /dev/null +++ b/drive/platform-implementations/offset_servo.cpp @@ -0,0 +1,24 @@ +#include "../include/offset_servo.hpp" +#include +#include + +namespace sjsu::drive { + +hal::result offset_servo::create(hal::servo& p_servo, hal::degrees p_offset) { + return offset_servo(p_servo, p_offset); +} +hal::degrees offset_servo::get_offset() { + return m_offset; +} + +void offset_servo::set_offset(hal::degrees p_offset) { + m_offset = p_offset; +} +offset_servo::offset_servo(hal::servo& p_servo, hal::degrees p_offset) : m_servo(&p_servo), m_offset(p_offset) {} + + +hal::result offset_servo::driver_position(hal::degrees p_position) { + return HAL_CHECK(m_servo->position(p_position + m_offset)); +} + +} diff --git a/drive/platform-implementations/offset_servo.hpp b/drive/platform-implementations/offset_servo.hpp deleted file mode 100644 index be16235..0000000 --- a/drive/platform-implementations/offset_servo.hpp +++ /dev/null @@ -1,35 +0,0 @@ -#pragma once - -#include -#include - -namespace sjsu::drive { - -class offset_servo : public hal::servo { -public: - static hal::result create(hal::servo& p_servo, hal::degrees p_offset) { - return offset_servo(p_servo, p_offset); - } - - hal::degrees get_offset() { - return m_offset; - } - - void set_offset(hal::degrees p_offset) { - m_offset = p_offset; - } - -private: - offset_servo(hal::servo& p_servo, hal::degrees p_offset) : m_servo(&p_servo), m_offset(p_offset) - { - } - - hal::result driver_position(hal::degrees p_position) override { - return HAL_CHECK(m_servo->position(p_position + m_offset)); - } - - hal::degrees m_offset; - hal::servo* m_servo = nullptr; -}; - -} diff --git a/drive/platform-implementations/print_mission_control.hpp b/drive/platform-implementations/print_mission_control.hpp deleted file mode 100644 index 14d00ec..0000000 --- a/drive/platform-implementations/print_mission_control.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -#include "mission_control.hpp" - -#include -#include -#include -#include -#include -#include - -namespace sjsu::drive{ - -class print_mission_control : public mission_control -{ -public: - - static hal::result create(hal::serial& p_console) - { - return print_mission_control(p_console); - } - - hal::result get_command(hal::function_ref p_timeout) - { - return impl_get_command(p_timeout); - } - -private: - - print_mission_control(hal::serial& p_console) : m_console(&p_console) - { - } - - hal::result impl_get_command(hal::function_ref p_timeout) { - hal::print(*m_console, "getting command"); - return mc_commands{}; - } - hal::serial* m_console; -}; - -} diff --git a/drive/platform-implementations/print_motor.hpp b/drive/platform-implementations/print_motor.hpp deleted file mode 100644 index 7172c5b..0000000 --- a/drive/platform-implementations/print_motor.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace sjsu::drive { - -class print_motor : public hal::motor { -public: - static hal::result create(hal::serial& p_console) { - return print_motor(p_console); - } - -private: - print_motor(hal::serial& p_console) : m_console(&p_console) - { - } - - hal::result driver_power(float p_speed) override { - hal::print<20>(*m_console, "speed: %f\n", p_speed); - return hal::motor::power_t{}; - } - - hal::serial* m_console; -}; - -} diff --git a/drive/platform-implementations/print_servo.hpp b/drive/platform-implementations/print_servo.hpp deleted file mode 100644 index ceccee4..0000000 --- a/drive/platform-implementations/print_servo.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace sjsu::drive { - -class print_servo : public hal::servo { -public: - static hal::result create(hal::serial& p_console) { - return print_servo(p_console); - } - -private: - print_servo(hal::serial& p_console) : m_console(&p_console) - { - } - - hal::result driver_position(hal::degrees p_position) override { - hal::print<20>(*m_console, "position: %f\n", static_cast(p_position)); - return hal::servo::position_t{}; - } - - hal::serial* m_console; -}; - -} diff --git a/drive/platform-implementations/print_speed_sensor.hpp b/drive/platform-implementations/print_speed_sensor.hpp deleted file mode 100644 index 74196c4..0000000 --- a/drive/platform-implementations/print_speed_sensor.hpp +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include "speed_sensor.hpp" -#include -#include -#include - -namespace sjsu::drive { - -class print_speed_sensor : public speed_sensor { -public: - - static hal::result make_speed_sensor(hal::serial& p_console) { - return print_speed_sensor(p_console); - } - -private: - - print_speed_sensor(hal::serial& p_console) - : m_console(&p_console) - { - } - - hal::result driver_read() { - using namespace hal::literals; - - auto speed = 0.0_rpm; - hal::print<100>(*m_console, "speed read: %f\n", speed); - - return speed_sensor::read_t{.speed = 0.0_rpm}; - } - hal::serial* m_console; -}; -} diff --git a/drive/platform-implementations/serial_mission_control.cpp b/drive/platform-implementations/serial_mission_control.cpp new file mode 100644 index 0000000..a0b12bd --- /dev/null +++ b/drive/platform-implementations/serial_mission_control.cpp @@ -0,0 +1,48 @@ +#include + +#include + +#include "../include/serial_mission_control.hpp" + +namespace sjsu::drive { + +serial_mission_control::serial_mission_control(hal::serial& p_console) : +m_console(p_console) {} + +void serial_mission_control::parse_commands(std::string_view p_commands_json) { + + static constexpr int expected_number_of_arguments = 6; + mc_commands commands; + + int actual_arguments = sscanf(p_commands_json.data(), + kResponseBodyFormat, + &commands.heartbeat_count, + &commands.is_operational, + &commands.wheel_orientation, + &commands.mode, + &commands.speed, + &commands.angle); + if (actual_arguments != expected_number_of_arguments) { + hal::print<200>(m_console, + "Received %d arguments, expected %d\n", + actual_arguments, + expected_number_of_arguments); + } + m_commands = commands; +} + +hal::result serial_mission_control::impl_get_command( + hal::function_ref p_timeout) +{ + std::array buffer{}; + auto received = HAL_CHECK(m_console.read(buffer)).data; + if (received.size() != 0) { + auto response = std::string_view( + reinterpret_cast(received.data()), received.size()); + // hal::print<200>( + // m_console, "%.*s", static_cast(response.size()), response.data()); + parse_commands(response); + } + return m_commands; + } +} // namespace drive \ No newline at end of file diff --git a/drive/platforms/lpc4078.cpp b/drive/platforms/lpc4078.cpp index f6f2508..a68ebcd 100644 --- a/drive/platforms/lpc4078.cpp +++ b/drive/platforms/lpc4078.cpp @@ -14,18 +14,14 @@ #include #include -#include "../platform-implementations/drc_speed_sensor.cpp" +#include "../include/drc_speed_sensor.hpp" -#include "../platform-implementations/esp8266_mission_control.cpp" -#include "../platform-implementations/mission_control.hpp" +#include "../include/mission_control.hpp" +#include "../include/esp8266_mission_control.hpp" #include "../applications/application.hpp" #include "../platform-implementations/home.hpp" -#include "../platform-implementations/offset_servo.hpp" +#include "../include/offset_servo.hpp" #include "../platform-implementations/helper.hpp" -#include "../platform-implementations/print_mission_control.hpp" -#include "../platform-implementations/print_motor.hpp" -#include "../platform-implementations/print_servo.hpp" -#include "../platform-implementations/print_speed_sensor.hpp" namespace sjsu::drive { @@ -133,7 +129,7 @@ hal::result initialize_platform() // &extra_home, }; - HAL_CHECK(home(homing_structs, counter, &uart0)); + HAL_CHECK(home(homing_structs, counter)); // hal::print<100>(uart0, "right offset: %f", right_home.servo->get_offset()); // hal::print<100>(uart0, "left offset: %f", left_home.servo->get_offset()); // hal::print<100>(uart0, "back offset: %f", back_home.servo->get_offset()); @@ -197,14 +193,19 @@ hal::result initialize_platform() auto timeout = hal::create_timeout(counter, 10s); static auto esp8266 = HAL_CHECK(hal::esp8266::at::create(uart1, timeout)); auto mc_timeout = hal::create_timeout(counter, 10s); - static auto esp_mission_control = sjsu::drive::esp8266_mission_control::create(esp8266, - uart0, ssid, password, socket_config, - ip, mc_timeout, buffer, get_request); + esp8266_mission_control::create_t create_mission_control{.esp8266 = esp8266, + .console = uart0, + .ssid = ssid, + .password = password, + .config = socket_config, + .ip = ip, + .buffer = buffer, + .get_request = get_request}; + static auto esp_mission_control = esp8266_mission_control::create(create_mission_control, mc_timeout); + while(esp_mission_control.has_error()) { mc_timeout = hal::create_timeout(counter, 30s); - esp_mission_control = sjsu::drive::esp8266_mission_control::create(esp8266, - uart0, ssid, password, socket_config, - ip, mc_timeout, buffer, get_request); + esp_mission_control = esp8266_mission_control::create(create_mission_control, mc_timeout); } static auto drive_mission_control = esp_mission_control.value(); From b9a4de35a54d7b7a310735bdfbe1413df3dd3802 Mon Sep 17 00:00:00 2001 From: Coreyboy1820 <92711317+Coreyboy1820@users.noreply.github.com> Date: Wed, 20 Dec 2023 13:40:03 -0800 Subject: [PATCH 2/6] finished formatting all files (#102) --- drive/applications/application.cpp | 18 +- drive/applications/application.hpp | 14 +- drive/dto/drive.hpp | 5 +- drive/dto/motor_feedback.hpp | 43 +- drive/implementations/command_lerper.cpp | 18 +- drive/implementations/mode_select.hpp | 7 +- drive/implementations/mode_switcher.cpp | 78 ++-- drive/implementations/rules_engine.hpp | 5 +- drive/implementations/steer_modes.hpp | 34 +- drive/implementations/tri_wheel_router.cpp | 90 ++-- drive/include/command_lerper.hpp | 2 +- drive/include/drc_speed_sensor.hpp | 13 +- drive/include/esp8266_mission_control.hpp | 144 +++--- drive/include/mission_control.hpp | 10 +- drive/include/mode_switcher.hpp | 4 +- drive/include/offset_servo.hpp | 27 +- drive/include/serial_mission_control.hpp | 20 +- drive/include/speed_sensor.hpp | 27 +- drive/include/tri_wheel_router.hpp | 8 +- .../drc_speed_sensor.cpp | 26 +- .../esp8266_mission_control.cpp | 414 +++++++++--------- drive/platform-implementations/home.hpp | 88 ++-- .../platform-implementations/offset_servo.cpp | 33 +- .../serial_mission_control.cpp | 20 +- drive/platforms/lpc4078.cpp | 295 +++++++------ 25 files changed, 764 insertions(+), 679 deletions(-) diff --git a/drive/applications/application.cpp b/drive/applications/application.cpp index 61fd291..2282892 100644 --- a/drive/applications/application.cpp +++ b/drive/applications/application.cpp @@ -2,9 +2,9 @@ #include "../dto/motor_feedback.hpp" +#include "../implementations/mode_select.hpp" #include "../implementations/rules_engine.hpp" #include "../include/command_lerper.hpp" -#include "../implementations/mode_select.hpp" #include "../include/mode_switcher.hpp" #include "../include/tri_wheel_router.hpp" @@ -26,7 +26,7 @@ hal::status application(application_framework& p_framework) auto& clock = *p_framework.clock; auto loop_count = 0; - sjsu::drive::tri_wheel_router tri_wheel{back_leg, right_leg, left_leg}; + sjsu::drive::tri_wheel_router tri_wheel{ back_leg, right_leg, left_leg }; sjsu::drive::mission_control::mc_commands commands; sjsu::drive::motor_feedback motor_speeds; sjsu::drive::tri_wheel_router_arguments arguments; @@ -38,20 +38,20 @@ hal::status application(application_framework& p_framework) HAL_CHECK(hal::write(terminal, "Starting control loop...")); while (true) { - if(loop_count==10) { + if (loop_count == 10) { auto timeout = hal::create_timeout(clock, 1s); commands = mission_control.get_command(timeout).value(); - loop_count=0; + loop_count = 0; } loop_count++; motor_speeds = HAL_CHECK(tri_wheel.get_motor_feedback()); - + commands = sjsu::drive::validate_commands(commands); - commands = mode_switcher.switch_steer_mode( - commands, arguments, motor_speeds); + commands = + mode_switcher.switch_steer_mode(commands, arguments, motor_speeds); commands.speed = lerp.lerp(commands.speed); - + arguments = sjsu::drive::select_mode(commands); HAL_CHECK(tri_wheel.move(arguments, clock)); hal::delay(clock, 8ms); @@ -60,4 +60,4 @@ hal::status application(application_framework& p_framework) return hal::success(); } -} +} // namespace sjsu::drive diff --git a/drive/applications/application.hpp b/drive/applications/application.hpp index fb96671..112d112 100644 --- a/drive/applications/application.hpp +++ b/drive/applications/application.hpp @@ -1,18 +1,18 @@ #pragma once +#include "../include/mission_control.hpp" +#include "../include/offset_servo.hpp" +#include "../include/speed_sensor.hpp" +#include #include #include #include -#include #include #include -#include -#include "../include/speed_sensor.hpp" -#include "../include/mission_control.hpp" -#include "../include/offset_servo.hpp" namespace sjsu::drive { -struct leg { +struct leg +{ hal::servo* steer; hal::motor* propulsion; speed_sensor* steer_speed_sensor; @@ -36,4 +36,4 @@ hal::status initialize_processor(); hal::result initialize_platform(); hal::status application(application_framework& p_framework); -} +} // namespace sjsu::drive diff --git a/drive/dto/drive.hpp b/drive/dto/drive.hpp index c724a8f..771d7de 100644 --- a/drive/dto/drive.hpp +++ b/drive/dto/drive.hpp @@ -12,7 +12,8 @@ struct drive_arguments { hal::rpm speed = 0.0_rpm; hal::degrees angle = 0.0_deg; - void print(hal::serial& p_console) { + void print(hal::serial& p_console) + { hal::print<100>(p_console, "speed: %f\n angle: %f\n", speed, angle); } }; @@ -24,4 +25,4 @@ struct tri_wheel_router_arguments drive_arguments back{}; }; -} // namespace Drive +} // namespace sjsu::drive diff --git a/drive/dto/motor_feedback.hpp b/drive/dto/motor_feedback.hpp index 004e469..9dd3b46 100644 --- a/drive/dto/motor_feedback.hpp +++ b/drive/dto/motor_feedback.hpp @@ -1,25 +1,32 @@ #pragma once -#include #include +#include -namespace sjsu::drive +namespace sjsu::drive { +struct motor_feedback { - struct motor_feedback - { - float left_steer_speed = 5.0; - float right_steer_speed = 5.0; - float back_steer_speed = 5.0; - float left_drive_speed = 5.0; - float right_drive_speed = 5.0; - float back_drive_speed = 5.0; + float left_steer_speed = 5.0; + float right_steer_speed = 5.0; + float back_steer_speed = 5.0; + float left_drive_speed = 5.0; + float right_drive_speed = 5.0; + float back_drive_speed = 5.0; - hal::status Print(hal::serial& terminal) - { - hal::print<50>(terminal, "\nSteers: left speed: %f right speed: %f back speed: %f Drives: Steers: left speed: %f right speed: %f back speed: %f\n", - static_cast(left_steer_speed), static_cast(right_steer_speed), static_cast(back_steer_speed),static_cast(left_drive_speed), static_cast(right_drive_speed), static_cast(back_drive_speed)); - return hal::success(); - } - }; + hal::status Print(hal::serial& terminal) + { + hal::print<50>( + terminal, + "\nSteers: left speed: %f right speed: %f back speed: %f Drives: Steers: " + "left speed: %f right speed: %f back speed: %f\n", + static_cast(left_steer_speed), + static_cast(right_steer_speed), + static_cast(back_steer_speed), + static_cast(left_drive_speed), + static_cast(right_drive_speed), + static_cast(back_drive_speed)); + return hal::success(); + } +}; -} \ No newline at end of file +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/implementations/command_lerper.cpp b/drive/implementations/command_lerper.cpp index ccc09ea..6434b85 100644 --- a/drive/implementations/command_lerper.cpp +++ b/drive/implementations/command_lerper.cpp @@ -1,15 +1,13 @@ -#include "../dto/drive.hpp" #include "../include/command_lerper.hpp" +#include "../dto/drive.hpp" namespace sjsu::drive { - int command_lerper::lerp(float p_speed) - { - m_previous_speed = - static_cast(std::lerp(m_previous_speed, - p_speed, - m_lerp_speed)); - return m_previous_speed; - } -} // namespace drive +int command_lerper::lerp(float p_speed) +{ + m_previous_speed = + static_cast(std::lerp(m_previous_speed, p_speed, m_lerp_speed)); + return m_previous_speed; +} +} // namespace sjsu::drive diff --git a/drive/implementations/mode_select.hpp b/drive/implementations/mode_select.hpp index 7a05901..5b1621d 100644 --- a/drive/implementations/mode_select.hpp +++ b/drive/implementations/mode_select.hpp @@ -1,12 +1,13 @@ #pragma once #include "../dto/drive.hpp" +#include "../include/mission_control.hpp" #include "../include/mode_switcher.hpp" #include "steer_modes.hpp" -#include "../include/mission_control.hpp" namespace sjsu::drive { -inline tri_wheel_router_arguments select_mode(mission_control::mc_commands p_commands) +inline tri_wheel_router_arguments select_mode( + mission_control::mc_commands p_commands) { switch (p_commands.mode) { case 'D': @@ -26,4 +27,4 @@ inline tri_wheel_router_arguments select_mode(mission_control::mc_commands p_com } return tri_wheel_router_arguments{}; } -} // namespace Drive +} // namespace sjsu::drive diff --git a/drive/implementations/mode_switcher.cpp b/drive/implementations/mode_switcher.cpp index cf06781..910b876 100644 --- a/drive/implementations/mode_switcher.cpp +++ b/drive/implementations/mode_switcher.cpp @@ -1,49 +1,49 @@ +#include "../include/mode_switcher.hpp" #include "../dto/drive.hpp" #include "../dto/motor_feedback.hpp" #include "../include/mission_control.hpp" -#include "../include/mode_switcher.hpp" namespace sjsu::drive { - mission_control::mc_commands mode_switch::switch_steer_mode( - mission_control::mc_commands p_commands, - tri_wheel_router_arguments p_previous_arguments, - motor_feedback p_current_motor_feedback) - { - bool hubs_stopped((p_previous_arguments.left.speed >= -0.01f && - p_previous_arguments.left.speed <= 0.01f) && - (p_previous_arguments.right.speed >= -0.01f && - p_previous_arguments.right.speed <= 0.01f) && - (p_previous_arguments.back.speed >= -0.01f && - p_previous_arguments.back.speed <= 0.01f)); +mission_control::mc_commands mode_switch::switch_steer_mode( + mission_control::mc_commands p_commands, + tri_wheel_router_arguments p_previous_arguments, + motor_feedback p_current_motor_feedback) +{ + bool hubs_stopped((p_previous_arguments.left.speed >= -0.01f && + p_previous_arguments.left.speed <= 0.01f) && + (p_previous_arguments.right.speed >= -0.01f && + p_previous_arguments.right.speed <= 0.01f) && + (p_previous_arguments.back.speed >= -0.01f && + p_previous_arguments.back.speed <= 0.01f)); - bool steers_stopped((p_current_motor_feedback.left_steer_speed >= -0.01f && - p_current_motor_feedback.left_steer_speed <= 0.01f) && - (p_current_motor_feedback.right_steer_speed >= -0.01f && - p_current_motor_feedback.right_steer_speed <= 0.01f) && - (p_current_motor_feedback.back_steer_speed >= -0.01f && - p_current_motor_feedback.back_steer_speed <= 0.01f)); - - if (m_previous_mode != p_commands.mode) { - m_in_the_middle_of_switching_modes = true; - m_skip_once = true; - } + bool steers_stopped((p_current_motor_feedback.left_steer_speed >= -0.01f && + p_current_motor_feedback.left_steer_speed <= 0.01f) && + (p_current_motor_feedback.right_steer_speed >= -0.01f && + p_current_motor_feedback.right_steer_speed <= 0.01f) && + (p_current_motor_feedback.back_steer_speed >= -0.01f && + p_current_motor_feedback.back_steer_speed <= 0.01f)); - if (m_in_the_middle_of_switching_modes) { - p_commands.speed = 0; - if (!hubs_stopped) { - p_commands.mode = m_previous_mode; - return p_commands; - } // hubs must be stopped to pass here + if (m_previous_mode != p_commands.mode) { + m_in_the_middle_of_switching_modes = true; + m_skip_once = true; + } - else if (!m_skip_once && steers_stopped) { - m_in_the_middle_of_switching_modes = false; - m_skip_once = true; - } // only once steer motors have stopped moving after hubs stopped will - // we exit switching modes - m_previous_mode = p_commands.mode; - m_skip_once = false; - } + if (m_in_the_middle_of_switching_modes) { + p_commands.speed = 0; + if (!hubs_stopped) { + p_commands.mode = m_previous_mode; + return p_commands; + } // hubs must be stopped to pass here - return p_commands; + else if (!m_skip_once && steers_stopped) { + m_in_the_middle_of_switching_modes = false; + m_skip_once = true; + } // only once steer motors have stopped moving after hubs stopped will + // we exit switching modes + m_previous_mode = p_commands.mode; + m_skip_once = false; } -} \ No newline at end of file + + return p_commands; +} +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/implementations/rules_engine.hpp b/drive/implementations/rules_engine.hpp index 3e25862..6f9ef31 100644 --- a/drive/implementations/rules_engine.hpp +++ b/drive/implementations/rules_engine.hpp @@ -3,7 +3,8 @@ #include "../include/mission_control.hpp" namespace sjsu::drive { -inline mission_control::mc_commands validate_commands(mission_control::mc_commands p_commands) +inline mission_control::mc_commands validate_commands( + mission_control::mc_commands p_commands) { static constexpr int kMaxSpeed = 100; static constexpr int kMaxAngle = 12; @@ -22,4 +23,4 @@ inline mission_control::mc_commands validate_commands(mission_control::mc_comman return p_commands; } -} // namespace Drive \ No newline at end of file +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/implementations/steer_modes.hpp b/drive/implementations/steer_modes.hpp index a51b07f..6536f9f 100644 --- a/drive/implementations/steer_modes.hpp +++ b/drive/implementations/steer_modes.hpp @@ -2,21 +2,21 @@ #include #include -#include "../include/mission_control.hpp" #include "../dto/drive.hpp" +#include "../include/mission_control.hpp" namespace sjsu::drive { -static constexpr float left_leg_drive_offset = 37; // 41 -static constexpr float right_leg_drive_offset = 199.5; // 200 -static constexpr float back_leg_drive_offset = 117; // 122 +static constexpr float left_leg_drive_offset = 37; // 41 +static constexpr float right_leg_drive_offset = 199.5; // 200 +static constexpr float back_leg_drive_offset = 117; // 122 static constexpr float left_leg_spin_offset = 242; static constexpr float right_leg_spin_offset = 0; static constexpr float back_leg_spin_offset = 0; - -inline tri_wheel_router_arguments spin_steering(mission_control::mc_commands p_commands) +inline tri_wheel_router_arguments spin_steering( + mission_control::mc_commands p_commands) { tri_wheel_router_arguments temp; temp.back.angle = back_leg_spin_offset; @@ -29,7 +29,8 @@ inline tri_wheel_router_arguments spin_steering(mission_control::mc_commands p_c return temp; } -inline tri_wheel_router_arguments translate_steering(mission_control::mc_commands p_commands) +inline tri_wheel_router_arguments translate_steering( + mission_control::mc_commands p_commands) { tri_wheel_router_arguments steer_arguments; @@ -50,9 +51,10 @@ inline tri_wheel_router_arguments translate_steering(mission_control::mc_command /// Ackerman steering equation to compute outter wheel angle inline float calculate_ackerman(float p_outter_wheel_angle) { - float inner_wheel_angle = float(-.583 + 1.97 * abs(int(p_outter_wheel_angle)) + - -.224 * pow(abs(int(p_outter_wheel_angle)), 2) + - .0278 * pow(abs(int(p_outter_wheel_angle)), 3)); + float inner_wheel_angle = + float(-.583 + 1.97 * abs(int(p_outter_wheel_angle)) + + -.224 * pow(abs(int(p_outter_wheel_angle)), 2) + + .0278 * pow(abs(int(p_outter_wheel_angle)), 3)); return (p_outter_wheel_angle > 0) ? inner_wheel_angle : -inner_wheel_angle; } @@ -92,7 +94,8 @@ inline float get_back_wheel_hub_speed(float p_outter_wheel_speed, return (p_outter_wheel_speed * ratio); } -inline tri_wheel_router_arguments drive_steering(mission_control::mc_commands p_commands) +inline tri_wheel_router_arguments drive_steering( + mission_control::mc_commands p_commands) { float outter_wheel_angle = 0.0, back_wheel_angle = 0.0; tri_wheel_router_arguments steer_arguments; @@ -112,9 +115,10 @@ inline tri_wheel_router_arguments drive_steering(mission_control::mc_commands p_ steer_arguments.right.angle = 0; } else { outter_wheel_angle = -outter_wheel_angle; - back_wheel_angle = float(-.0474 + -1.93 * abs(static_cast(outter_wheel_angle)) + - -.0813 * pow(abs(static_cast(outter_wheel_angle)), 2) + - .000555 * pow(abs(static_cast(outter_wheel_angle)), 3)); + back_wheel_angle = + float(-.0474 + -1.93 * abs(static_cast(outter_wheel_angle)) + + -.0813 * pow(abs(static_cast(outter_wheel_angle)), 2) + + .000555 * pow(abs(static_cast(outter_wheel_angle)), 3)); back_wheel_angle = (outter_wheel_angle > 0) ? -back_wheel_angle : back_wheel_angle; steer_arguments.back.angle = back_wheel_angle; @@ -147,4 +151,4 @@ inline tri_wheel_router_arguments drive_steering(mission_control::mc_commands p_ return steer_arguments; } -} // namespace Drive +} // namespace sjsu::drive diff --git a/drive/implementations/tri_wheel_router.cpp b/drive/implementations/tri_wheel_router.cpp index b7e4855..78cc67f 100644 --- a/drive/implementations/tri_wheel_router.cpp +++ b/drive/implementations/tri_wheel_router.cpp @@ -1,52 +1,60 @@ #include -#include #include #include #include +#include +#include "../applications/application.hpp" #include "../dto/drive.hpp" #include "../dto/motor_feedback.hpp" -#include "../applications/application.hpp" #include "../include/tri_wheel_router.hpp" namespace sjsu::drive { - tri_wheel_router::tri_wheel_router(leg& p_back, leg& p_right, leg& p_left) - : m_left(p_left) - , m_back(p_back) - , m_right(p_right) - { - } - - hal::status tri_wheel_router::move(tri_wheel_router_arguments p_tri_wheel_arguments, - hal::steady_clock& p_clock) - { - HAL_CHECK(m_left.steer->position(-p_tri_wheel_arguments.left.angle)); - HAL_CHECK(m_left.propulsion->power(p_tri_wheel_arguments.left.speed/100)); - - HAL_CHECK(m_right.steer->position(-p_tri_wheel_arguments.right.angle)); - HAL_CHECK(m_right.propulsion->power(-p_tri_wheel_arguments.right.speed/100)); - - HAL_CHECK(m_back.steer->position(-p_tri_wheel_arguments.back.angle)); - HAL_CHECK(m_back.propulsion->power(p_tri_wheel_arguments.back.speed/100)); - - return hal::success(); - } - - hal::result tri_wheel_router::get_motor_feedback() - { - using namespace std::chrono_literals; - using namespace hal::literals; - motor_feedback motor_speeds; - - motor_speeds.left_drive_speed = HAL_CHECK(m_left.propulsion_speed_sensor->read()).speed; - motor_speeds.left_steer_speed = HAL_CHECK(m_left.steer_speed_sensor->read()).speed; - motor_speeds.back_drive_speed = HAL_CHECK(m_back.propulsion_speed_sensor->read()).speed; - motor_speeds.back_steer_speed = HAL_CHECK(m_back.steer_speed_sensor->read()).speed; - motor_speeds.right_drive_speed = HAL_CHECK(m_right.propulsion_speed_sensor->read()).speed; - motor_speeds.right_steer_speed = HAL_CHECK(m_right.steer_speed_sensor->read()).speed; - - return motor_speeds; - } - -} \ No newline at end of file +tri_wheel_router::tri_wheel_router(leg& p_back, leg& p_right, leg& p_left) + : m_left(p_left) + , m_back(p_back) + , m_right(p_right) +{ +} + +hal::status tri_wheel_router::move( + tri_wheel_router_arguments p_tri_wheel_arguments, + hal::steady_clock& p_clock) +{ + HAL_CHECK(m_left.steer->position(-p_tri_wheel_arguments.left.angle)); + HAL_CHECK(m_left.propulsion->power(p_tri_wheel_arguments.left.speed / 100)); + + HAL_CHECK(m_right.steer->position(-p_tri_wheel_arguments.right.angle)); + HAL_CHECK( + m_right.propulsion->power(-p_tri_wheel_arguments.right.speed / 100)); + + HAL_CHECK(m_back.steer->position(-p_tri_wheel_arguments.back.angle)); + HAL_CHECK(m_back.propulsion->power(p_tri_wheel_arguments.back.speed / 100)); + + return hal::success(); +} + +hal::result tri_wheel_router::get_motor_feedback() +{ + using namespace std::chrono_literals; + using namespace hal::literals; + motor_feedback motor_speeds; + + motor_speeds.left_drive_speed = + HAL_CHECK(m_left.propulsion_speed_sensor->read()).speed; + motor_speeds.left_steer_speed = + HAL_CHECK(m_left.steer_speed_sensor->read()).speed; + motor_speeds.back_drive_speed = + HAL_CHECK(m_back.propulsion_speed_sensor->read()).speed; + motor_speeds.back_steer_speed = + HAL_CHECK(m_back.steer_speed_sensor->read()).speed; + motor_speeds.right_drive_speed = + HAL_CHECK(m_right.propulsion_speed_sensor->read()).speed; + motor_speeds.right_steer_speed = + HAL_CHECK(m_right.steer_speed_sensor->read()).speed; + + return motor_speeds; +} + +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/include/command_lerper.hpp b/drive/include/command_lerper.hpp index 9c083e4..7285a5b 100644 --- a/drive/include/command_lerper.hpp +++ b/drive/include/command_lerper.hpp @@ -12,4 +12,4 @@ class command_lerper static constexpr float m_lerp_speed = 0.1f; float m_previous_speed = 0; }; -} // namespace drive +} // namespace sjsu::drive diff --git a/drive/include/drc_speed_sensor.hpp b/drive/include/drc_speed_sensor.hpp index 349033b..5843f5e 100644 --- a/drive/include/drc_speed_sensor.hpp +++ b/drive/include/drc_speed_sensor.hpp @@ -9,11 +9,12 @@ class drc_speed_sensor; hal::result make_speed_sensor(hal::rmd::drc& p_drc); -class drc_speed_sensor : public speed_sensor { +class drc_speed_sensor : public speed_sensor +{ private: - drc_speed_sensor(hal::rmd::drc& p_drc); - hal::result driver_read() override; - friend hal::result make_speed_sensor(hal::rmd::drc& p_drc); - hal::rmd::drc* m_drc = nullptr; + drc_speed_sensor(hal::rmd::drc& p_drc); + hal::result driver_read() override; + friend hal::result make_speed_sensor(hal::rmd::drc& p_drc); + hal::rmd::drc* m_drc = nullptr; }; -} \ No newline at end of file +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/include/esp8266_mission_control.hpp b/drive/include/esp8266_mission_control.hpp index 02624c9..680a538 100644 --- a/drive/include/esp8266_mission_control.hpp +++ b/drive/include/esp8266_mission_control.hpp @@ -4,67 +4,67 @@ #include #include +#include "mission_control.hpp" #include #include #include #include #include #include -#include "mission_control.hpp" -namespace sjsu::drive{ +namespace sjsu::drive { -class esp8266_mission_control : public mission_control +class esp8266_mission_control : public mission_control { public: + struct create_t + { + hal::esp8266::at& esp8266; + hal::serial& console; + const std::string_view ssid; + const std::string_view password; + const hal::esp8266::at::socket_config& config; + const std::string_view ip; + std::span buffer; + std::string_view get_request; + }; + + struct http_header_parser_t + { + hal::stream_find find_header_start; + hal::stream_find find_content_length; + hal::stream_parse parse_content_length; + hal::stream_find find_end_of_header; + }; - struct create_t { - hal::esp8266::at& esp8266; - hal::serial& console; - const std::string_view ssid; - const std::string_view password; - const hal::esp8266::at::socket_config& config; - const std::string_view ip; - std::span buffer; - std::string_view get_request; - }; - - struct http_header_parser_t - { - hal::stream_find find_header_start; - hal::stream_find find_content_length; - hal::stream_parse parse_content_length; - hal::stream_find find_end_of_header; - }; - - enum class connection_state - { - check_ap_connection, - connecting_to_ap, - set_ip_address, - check_server_connection, - connecting_to_server, - connection_established, - }; - - [[nodiscard]] static hal::result create(create_t p_create, hal::timeout auto& p_timeout) + enum class connection_state + { + check_ap_connection, + connecting_to_ap, + set_ip_address, + check_server_connection, + connecting_to_server, + connection_established, + }; + + [[nodiscard]] static hal::result create( + create_t p_create, + hal::timeout auto& p_timeout) { esp8266_mission_control esp_mission_control(p_create.esp8266, - p_create.console, - p_create.ssid, - p_create.password, - p_create.config, - p_create.ip, - p_create.buffer, - p_create.get_request); + p_create.console, + p_create.ssid, + p_create.password, + p_create.config, + p_create.ip, + p_create.buffer, + p_create.get_request); HAL_CHECK(esp_mission_control.establish_connection(p_timeout)); return esp_mission_control; } - private: - esp8266_mission_control(hal::esp8266::at& p_esp8266, hal::serial& p_console, const std::string_view p_ssid, @@ -74,34 +74,36 @@ class esp8266_mission_control : public mission_control std::span p_buffer, std::string_view p_get_request); - hal::result impl_get_command(hal::function_ref p_timeout) override; - - void parse_commands(); - - std::string_view to_string_view(std::span p_span); - - hal::status establish_connection(hal::function_ref p_timeout); - - http_header_parser_t new_http_header_parser(); - - mc_commands m_commands{}; - http_header_parser_t m_http_header_parser; - hal::stream_fill m_fill_payload; - const hal::esp8266::at::socket_config& m_config; - hal::esp8266::at* m_esp8266; - hal::serial* m_console; - std::string_view m_ssid; - std::string_view m_password; - std::string_view m_get_request; - std::string_view m_ip; - std::span m_buffer; - std::array m_command_buffer; - size_t m_buffer_len; - size_t m_content_length; - bool m_write_error = false; - bool m_header_finished = false; - bool m_read_complete = true; - std::uint32_t m_missed_read = 0; + hal::result impl_get_command( + hal::function_ref p_timeout) override; + + void parse_commands(); + + std::string_view to_string_view(std::span p_span); + + hal::status establish_connection( + hal::function_ref p_timeout); + + http_header_parser_t new_http_header_parser(); + + mc_commands m_commands{}; + http_header_parser_t m_http_header_parser; + hal::stream_fill m_fill_payload; + const hal::esp8266::at::socket_config& m_config; + hal::esp8266::at* m_esp8266; + hal::serial* m_console; + std::string_view m_ssid; + std::string_view m_password; + std::string_view m_get_request; + std::string_view m_ip; + std::span m_buffer; + std::array m_command_buffer; + size_t m_buffer_len; + size_t m_content_length; + bool m_write_error = false; + bool m_header_finished = false; + bool m_read_complete = true; + std::uint32_t m_missed_read = 0; }; -} +} // namespace sjsu::drive diff --git a/drive/include/mission_control.hpp b/drive/include/mission_control.hpp index ac02b8e..54109bc 100644 --- a/drive/include/mission_control.hpp +++ b/drive/include/mission_control.hpp @@ -7,7 +7,7 @@ #include #include -namespace sjsu::drive{ +namespace sjsu::drive { static constexpr char kResponseBodyFormat[] = "{\"HB\":%d,\"IO\":%d,\"WO\":%d,\"DM\":\"%c\",\"CMD\":[%d,%d]}\n"; @@ -16,10 +16,9 @@ static constexpr char kGETRequestFormat[] = "drive?heartbeat_count=%d&is_operational=%d&wheel_orientation=%d&drive_mode=%" "c&speed=%d&angle=%d"; - class mission_control { - public: +public: struct mc_commands { char mode = 'D'; @@ -68,7 +67,8 @@ class mission_control * commands have been received, then this should return the default * initialized command. */ - hal::result get_command(hal::function_ref p_timeout) + hal::result get_command( + hal::function_ref p_timeout) { return impl_get_command(p_timeout); } @@ -80,4 +80,4 @@ class mission_control hal::function_ref p_timeout) = 0; }; -} \ No newline at end of file +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/include/mode_switcher.hpp b/drive/include/mode_switcher.hpp index b386f02..5614edf 100644 --- a/drive/include/mode_switcher.hpp +++ b/drive/include/mode_switcher.hpp @@ -15,13 +15,11 @@ class mode_switch motor_feedback p_current_motor_feedback); private: - char m_previous_mode = 'H'; // This is H for homing when the rover turns on, it makes sure that we // don't switch to drive mode and allow commands to be parsed through bool m_in_the_middle_of_switching_modes = true; bool m_skip_once = true; - }; -} // namespace Drive +} // namespace sjsu::drive diff --git a/drive/include/offset_servo.hpp b/drive/include/offset_servo.hpp index 9db0882..3590c3d 100644 --- a/drive/include/offset_servo.hpp +++ b/drive/include/offset_servo.hpp @@ -1,25 +1,28 @@ -#pragma once +#pragma once -#include #include +#include namespace sjsu::drive { -class offset_servo : public hal::servo { +class offset_servo : public hal::servo +{ public: - static hal::result create(hal::servo& p_servo, hal::degrees p_offset); + static hal::result create(hal::servo& p_servo, + hal::degrees p_offset); + + hal::degrees get_offset(); - hal::degrees get_offset(); + void set_offset(hal::degrees p_offset); - void set_offset(hal::degrees p_offset); - private: - offset_servo(hal::servo& p_servo, hal::degrees p_offset); + offset_servo(hal::servo& p_servo, hal::degrees p_offset); - hal::result driver_position(hal::degrees p_position) override; + hal::result driver_position( + hal::degrees p_position) override; - hal::degrees m_offset; - hal::servo* m_servo = nullptr; + hal::degrees m_offset; + hal::servo* m_servo = nullptr; }; -} +} // namespace sjsu::drive diff --git a/drive/include/serial_mission_control.hpp b/drive/include/serial_mission_control.hpp index f0aa5ac..2e9dc85 100644 --- a/drive/include/serial_mission_control.hpp +++ b/drive/include/serial_mission_control.hpp @@ -8,10 +8,11 @@ namespace sjsu::drive { -class serial_mission_control : public mission_control{ +class serial_mission_control : public mission_control +{ public: - -[[nodiscard]] static hal::result create(hal::serial& p_console) + [[nodiscard]] static hal::result create( + hal::serial& p_console) { serial_mission_control s_mission_control(p_console); @@ -19,15 +20,14 @@ class serial_mission_control : public mission_control{ } private: + serial_mission_control(hal::serial& p_console); - serial_mission_control(hal::serial& p_console); - - void parse_commands(std::string_view p_commands_json); + void parse_commands(std::string_view p_commands_json); - hal::result impl_get_command( + hal::result impl_get_command( hal::function_ref p_timeout) override; - hal::serial& m_console; - mc_commands m_commands; + hal::serial& m_console; + mc_commands m_commands; }; -} // namespace Drive \ No newline at end of file +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/include/speed_sensor.hpp b/drive/include/speed_sensor.hpp index c305d46..fb559db 100644 --- a/drive/include/speed_sensor.hpp +++ b/drive/include/speed_sensor.hpp @@ -4,21 +4,22 @@ namespace sjsu::drive { -class speed_sensor +class speed_sensor { - public: - struct read_t { - hal::rpm speed = 0; - }; +public: + struct read_t + { + hal::rpm speed = 0; + }; - [[nodiscard]] hal::result read() - { - return driver_read(); - } + [[nodiscard]] hal::result read() + { + return driver_read(); + } - virtual ~speed_sensor() = default; + virtual ~speed_sensor() = default; - private: - virtual hal::result driver_read() = 0; +private: + virtual hal::result driver_read() = 0; }; -} \ No newline at end of file +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/include/tri_wheel_router.hpp b/drive/include/tri_wheel_router.hpp index 1b1b45b..73171a2 100644 --- a/drive/include/tri_wheel_router.hpp +++ b/drive/include/tri_wheel_router.hpp @@ -1,20 +1,19 @@ #pragma once #include -#include #include #include #include +#include +#include "../applications/application.hpp" #include "../dto/drive.hpp" #include "../dto/motor_feedback.hpp" -#include "../applications/application.hpp" namespace sjsu::drive { class tri_wheel_router { public: - tri_wheel_router(leg& p_back, leg& p_right, leg& p_left); hal::status move(tri_wheel_router_arguments p_tri_wheel_arguments, @@ -23,11 +22,10 @@ class tri_wheel_router hal::result get_motor_feedback(); private: - // member variables leg& m_left; leg& m_back; leg& m_right; }; -} // namespace Drive +} // namespace sjsu::drive diff --git a/drive/platform-implementations/drc_speed_sensor.cpp b/drive/platform-implementations/drc_speed_sensor.cpp index 3545e8b..648306a 100644 --- a/drive/platform-implementations/drc_speed_sensor.cpp +++ b/drive/platform-implementations/drc_speed_sensor.cpp @@ -3,18 +3,20 @@ namespace sjsu::drive { - drc_speed_sensor::drc_speed_sensor(hal::rmd::drc& p_drc) - : m_drc(&p_drc) - { - } +drc_speed_sensor::drc_speed_sensor(hal::rmd::drc& p_drc) + : m_drc(&p_drc) +{ +} - hal::result drc_speed_sensor::driver_read() { - HAL_CHECK(m_drc->feedback_request(hal::rmd::drc::read::status_2)); +hal::result drc_speed_sensor::driver_read() +{ + HAL_CHECK(m_drc->feedback_request(hal::rmd::drc::read::status_2)); - return speed_sensor::read_t{.speed = m_drc->feedback().speed()}; - } + return speed_sensor::read_t{ .speed = m_drc->feedback().speed() }; +} - hal::result make_speed_sensor(hal::rmd::drc& p_drc) { - return drc_speed_sensor(p_drc); - } -} \ No newline at end of file +hal::result make_speed_sensor(hal::rmd::drc& p_drc) +{ + return drc_speed_sensor(p_drc); +} +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/platform-implementations/esp8266_mission_control.cpp b/drive/platform-implementations/esp8266_mission_control.cpp index 1e2f19a..30edf8f 100644 --- a/drive/platform-implementations/esp8266_mission_control.cpp +++ b/drive/platform-implementations/esp8266_mission_control.cpp @@ -9,235 +9,239 @@ #include #include -#include "../include/mission_control.hpp" #include "../include/esp8266_mission_control.hpp" +#include "../include/mission_control.hpp" namespace sjsu::drive { - - esp8266_mission_control::esp8266_mission_control(hal::esp8266::at& p_esp8266, - hal::serial& p_console, - const std::string_view p_ssid, - const std::string_view p_password, - const hal::esp8266::at::socket_config& p_config, - const std::string_view p_ip, - std::span p_buffer, - std::string_view p_get_request) - : m_esp8266(&p_esp8266) - , m_console(&p_console) - , m_ssid(p_ssid) - , m_password(p_password) - , m_config(p_config) - , m_ip(p_ip) - , m_buffer(p_buffer) - , m_get_request(p_get_request) - , m_fill_payload(hal::stream_fill(m_buffer)) - , m_http_header_parser(new_http_header_parser()) - , m_buffer_len(0) - { - } - - hal::result esp8266_mission_control::impl_get_command( - hal::function_ref p_timeout) - { - using namespace std::literals; - - // auto dumfuck = HAL_CHECK(m_esp8266->is_connected_to_server(p_timeout)); - if (m_write_error) { - hal::print(*m_console, "Reconnecting...\n"); - // Wait 1s before attempting to reconnect - auto result = establish_connection(p_timeout); - if (!result) { - hal::print(*m_console, "Failure!!!\n"); - return m_commands; - } - - hal::print(*m_console, "CONNECTION RE-ESTABLISHED!!\n"); - m_read_complete = true; - m_buffer_len = 0; - m_content_length = 0; - m_http_header_parser = new_http_header_parser(); - m_write_error = false; - m_header_finished = false; +esp8266_mission_control::esp8266_mission_control( + hal::esp8266::at& p_esp8266, + hal::serial& p_console, + const std::string_view p_ssid, + const std::string_view p_password, + const hal::esp8266::at::socket_config& p_config, + const std::string_view p_ip, + std::span p_buffer, + std::string_view p_get_request) + : m_esp8266(&p_esp8266) + , m_console(&p_console) + , m_ssid(p_ssid) + , m_password(p_password) + , m_config(p_config) + , m_ip(p_ip) + , m_buffer(p_buffer) + , m_get_request(p_get_request) + , m_fill_payload(hal::stream_fill(m_buffer)) + , m_http_header_parser(new_http_header_parser()) + , m_buffer_len(0) +{ +} + +hal::result +esp8266_mission_control::impl_get_command( + hal::function_ref p_timeout) +{ + using namespace std::literals; + + // auto dumfuck = HAL_CHECK(m_esp8266->is_connected_to_server(p_timeout)); + if (m_write_error) { + hal::print(*m_console, "Reconnecting...\n"); + // Wait 1s before attempting to reconnect + auto result = establish_connection(p_timeout); + if (!result) { + hal::print(*m_console, "Failure!!!\n"); + return m_commands; } - if (m_read_complete) { - // Send out HTTP GET request + hal::print(*m_console, "CONNECTION RE-ESTABLISHED!!\n"); + m_read_complete = true; + m_buffer_len = 0; + m_content_length = 0; + m_http_header_parser = new_http_header_parser(); + m_write_error = false; + m_header_finished = false; + } + if (m_read_complete) { - auto status = - m_esp8266->server_write(hal::as_bytes(m_get_request), p_timeout); + // Send out HTTP GET request - if (!status) { - hal::print(*m_console, "\nFailed to write to server!\n"); - hal::print(*m_console, m_get_request); - m_write_error = true; - return m_commands; - } + auto status = + m_esp8266->server_write(hal::as_bytes(m_get_request), p_timeout); - m_missed_read = 0; - m_read_complete = false; - m_header_finished = false; - } - auto received = HAL_CHECK(m_esp8266->server_read(m_buffer)).data; - auto remainder = received | m_http_header_parser.find_header_start | - m_http_header_parser.find_content_length | - m_http_header_parser.parse_content_length | - m_http_header_parser.find_end_of_header; - - m_missed_read++; - if (m_missed_read > 10) { - hal::print(*m_console, "READ MISS!!!\n"); + if (!status) { + hal::print(*m_console, "\nFailed to write to server!\n"); + hal::print(*m_console, m_get_request); m_write_error = true; - m_missed_read = 0; return m_commands; } - if (!m_header_finished && - hal::finished(m_http_header_parser.find_end_of_header)) { - m_content_length = m_http_header_parser.parse_content_length.value(); - m_header_finished = true; - std::fill(m_command_buffer.begin(), m_command_buffer.end(), 0); - remainder = remainder.subspan(1); - } - if (m_header_finished) { - // hal::print<128>(*m_console, " read miss = %u\n", m_missed_read); - - auto tmp = m_content_length - m_buffer_len; - auto byte_to_read = std::min((size_t)tmp, remainder.size()); - - for (int i = 0; i < byte_to_read; i++) { - m_command_buffer[m_buffer_len + i] = remainder[i]; - } - m_buffer_len += byte_to_read; - // hal::print<1024>(*m_console, - // "M header has finished, remainder size: %d, buffer " - // "data: %.*s, buffer len %d, content length %d\n", - // remainder.size(), - // m_buffer_len + 1, - // m_command_buffer.data(), - // m_buffer_len, - // m_content_length); - - if (m_buffer_len >= m_content_length) { - // hal::print(*m_console, "content length has been met \n"); - m_read_complete = true; - m_missed_read = 0; - m_buffer_len = 0; - parse_commands(); - m_http_header_parser = new_http_header_parser(); - } - } + m_missed_read = 0; + m_read_complete = false; + m_header_finished = false; + } + auto received = HAL_CHECK(m_esp8266->server_read(m_buffer)).data; + auto remainder = received | m_http_header_parser.find_header_start | + m_http_header_parser.find_content_length | + m_http_header_parser.parse_content_length | + m_http_header_parser.find_end_of_header; + + m_missed_read++; + if (m_missed_read > 10) { + hal::print(*m_console, "READ MISS!!!\n"); + m_write_error = true; + m_missed_read = 0; return m_commands; } - - void esp8266_mission_control::parse_commands() { - - auto result = to_string_view(m_command_buffer); - static constexpr int expected_number_of_arguments = 6; - mc_commands commands; - - int actual_arguments = sscanf(result.data(), - kResponseBodyFormat, - &commands.heartbeat_count, - &commands.is_operational, - &commands.wheel_orientation, - &commands.mode, - &commands.speed, - &commands.angle); - if (actual_arguments != expected_number_of_arguments) { - hal::print<200>(*m_console, - "Received %d arguments, expected %d\n", - actual_arguments, - expected_number_of_arguments); - } - - m_commands = commands; + if (!m_header_finished && + hal::finished(m_http_header_parser.find_end_of_header)) { + m_content_length = m_http_header_parser.parse_content_length.value(); + m_header_finished = true; + std::fill(m_command_buffer.begin(), m_command_buffer.end(), 0); + remainder = remainder.subspan(1); } - esp8266_mission_control::http_header_parser_t esp8266_mission_control::new_http_header_parser() - { - using namespace std::literals; - - return esp8266_mission_control::http_header_parser_t{ - .find_header_start = hal::stream_find(hal::as_bytes("HTTP/1.1 "sv)), - .find_content_length = - hal::stream_find(hal::as_bytes("Content-Length: "sv)), - .parse_content_length = hal::stream_parse(), - .find_end_of_header = hal::stream_find(hal::as_bytes("\r\n\r\n"sv)), - }; - } + if (m_header_finished) { + // hal::print<128>(*m_console, " read miss = %u\n", m_missed_read); + auto tmp = m_content_length - m_buffer_len; + auto byte_to_read = std::min((size_t)tmp, remainder.size()); - std::string_view esp8266_mission_control::to_string_view(std::span p_span) - { - return std::string_view(reinterpret_cast(p_span.data()), - p_span.size()); + for (int i = 0; i < byte_to_read; i++) { + m_command_buffer[m_buffer_len + i] = remainder[i]; + } + m_buffer_len += byte_to_read; + // hal::print<1024>(*m_console, + // "M header has finished, remainder size: %d, buffer " + // "data: %.*s, buffer len %d, content length %d\n", + // remainder.size(), + // m_buffer_len + 1, + // m_command_buffer.data(), + // m_buffer_len, + // m_content_length); + + if (m_buffer_len >= m_content_length) { + // hal::print(*m_console, "content length has been met \n"); + m_read_complete = true; + m_missed_read = 0; + m_buffer_len = 0; + parse_commands(); + m_http_header_parser = new_http_header_parser(); + } + } + return m_commands; +} + +void esp8266_mission_control::parse_commands() +{ + + auto result = to_string_view(m_command_buffer); + static constexpr int expected_number_of_arguments = 6; + mc_commands commands; + + int actual_arguments = sscanf(result.data(), + kResponseBodyFormat, + &commands.heartbeat_count, + &commands.is_operational, + &commands.wheel_orientation, + &commands.mode, + &commands.speed, + &commands.angle); + if (actual_arguments != expected_number_of_arguments) { + hal::print<200>(*m_console, + "Received %d arguments, expected %d\n", + actual_arguments, + expected_number_of_arguments); } - hal::status esp8266_mission_control::establish_connection(hal::function_ref p_timeout) - { - connection_state state = connection_state::check_ap_connection; - - while (state != connection_state::connection_established) { - switch (state) { - case connection_state::check_ap_connection: - hal::print(*m_console, "Checking if AP \""); - hal::print(*m_console, m_ssid); - hal::print(*m_console, "\" is connected... "); - if (HAL_CHECK(m_esp8266->is_connected_to_ap(p_timeout))) { - state = connection_state::check_server_connection; - hal::print(*m_console, "Connected!\n"); - } else { - state = connection_state::connecting_to_ap; - hal::print(*m_console, "NOT Connected!\n"); - } - break; - case connection_state::connecting_to_ap: - hal::print(*m_console, "Connecting to AP: \""); - hal::print(*m_console, m_ssid); - hal::print(*m_console, "\" ...\n"); - HAL_CHECK(m_esp8266->connect_to_ap(m_ssid, m_password, p_timeout)); - state = connection_state::set_ip_address; - break; - case connection_state::set_ip_address: - if (!m_ip.empty()) { - hal::print(*m_console, "Setting IP Address to: "); - hal::print(*m_console, m_ip); - hal::print(*m_console, " ...\n"); - HAL_CHECK(m_esp8266->set_ip_address(m_ip, p_timeout)); - } + m_commands = commands; +} + +esp8266_mission_control::http_header_parser_t +esp8266_mission_control::new_http_header_parser() +{ + using namespace std::literals; + + return esp8266_mission_control::http_header_parser_t{ + .find_header_start = hal::stream_find(hal::as_bytes("HTTP/1.1 "sv)), + .find_content_length = + hal::stream_find(hal::as_bytes("Content-Length: "sv)), + .parse_content_length = hal::stream_parse(), + .find_end_of_header = hal::stream_find(hal::as_bytes("\r\n\r\n"sv)), + }; +} + +std::string_view esp8266_mission_control::to_string_view( + std::span p_span) +{ + return std::string_view(reinterpret_cast(p_span.data()), + p_span.size()); +} + +hal::status esp8266_mission_control::establish_connection( + hal::function_ref p_timeout) +{ + connection_state state = connection_state::check_ap_connection; + + while (state != connection_state::connection_established) { + switch (state) { + case connection_state::check_ap_connection: + hal::print(*m_console, "Checking if AP \""); + hal::print(*m_console, m_ssid); + hal::print(*m_console, "\" is connected... "); + if (HAL_CHECK(m_esp8266->is_connected_to_ap(p_timeout))) { state = connection_state::check_server_connection; - break; - case connection_state::check_server_connection: - hal::print(*m_console, "Checking if server \""); - hal::print(*m_console, m_config.domain); - hal::print(*m_console, "\" is connected... \n"); - if (HAL_CHECK(m_esp8266->is_connected_to_server(p_timeout))) { - state = connection_state::connection_established; - hal::print(*m_console, "Connected!\n"); - } else { - state = connection_state::connecting_to_server; - hal::print(*m_console, "NOT Connected!\n"); - } - break; - case connection_state::connecting_to_server: - hal::print(*m_console, "Connecting to server: \""); - hal::print(*m_console, m_config.domain); - hal::print(*m_console, "\" ...\n"); - HAL_CHECK(m_esp8266->connect_to_server(m_config, p_timeout)); - hal::print(*m_console, "connected\n"); - state = connection_state::check_server_connection; - break; - case connection_state::connection_established: - // Do nothing, allow next iteration to break while loop - hal::print(*m_console, "Succesfully Connected."); - break; - default: + hal::print(*m_console, "Connected!\n"); + } else { state = connection_state::connecting_to_ap; - } + hal::print(*m_console, "NOT Connected!\n"); + } + break; + case connection_state::connecting_to_ap: + hal::print(*m_console, "Connecting to AP: \""); + hal::print(*m_console, m_ssid); + hal::print(*m_console, "\" ...\n"); + HAL_CHECK(m_esp8266->connect_to_ap(m_ssid, m_password, p_timeout)); + state = connection_state::set_ip_address; + break; + case connection_state::set_ip_address: + if (!m_ip.empty()) { + hal::print(*m_console, "Setting IP Address to: "); + hal::print(*m_console, m_ip); + hal::print(*m_console, " ...\n"); + HAL_CHECK(m_esp8266->set_ip_address(m_ip, p_timeout)); + } + state = connection_state::check_server_connection; + break; + case connection_state::check_server_connection: + hal::print(*m_console, "Checking if server \""); + hal::print(*m_console, m_config.domain); + hal::print(*m_console, "\" is connected... \n"); + if (HAL_CHECK(m_esp8266->is_connected_to_server(p_timeout))) { + state = connection_state::connection_established; + hal::print(*m_console, "Connected!\n"); + } else { + state = connection_state::connecting_to_server; + hal::print(*m_console, "NOT Connected!\n"); + } + break; + case connection_state::connecting_to_server: + hal::print(*m_console, "Connecting to server: \""); + hal::print(*m_console, m_config.domain); + hal::print(*m_console, "\" ...\n"); + HAL_CHECK(m_esp8266->connect_to_server(m_config, p_timeout)); + hal::print(*m_console, "connected\n"); + state = connection_state::check_server_connection; + break; + case connection_state::connection_established: + // Do nothing, allow next iteration to break while loop + hal::print(*m_console, "Succesfully Connected."); + break; + default: + state = connection_state::connecting_to_ap; } - - return hal::success(); } + return hal::success(); +} + } // namespace sjsu::drive \ No newline at end of file diff --git a/drive/platform-implementations/home.hpp b/drive/platform-implementations/home.hpp index 88113a2..b034647 100644 --- a/drive/platform-implementations/home.hpp +++ b/drive/platform-implementations/home.hpp @@ -1,15 +1,15 @@ #pragma once #include "../applications/application.hpp" -#include +#include "../include/offset_servo.hpp" #include -#include #include -#include "../include/offset_servo.hpp" +#include +#include -namespace sjsu::drive +namespace sjsu::drive { +struct homing { -struct homing { offset_servo* servo; hal::input_pin* magnet; bool homed = false; @@ -17,53 +17,55 @@ struct homing { // this function will return offsets in the same order of servos passed in inline hal::status home(std::span p_homing_structs, - hal::steady_clock& p_counter) { + hal::steady_clock& p_counter) +{ - using namespace std::chrono_literals; - using namespace hal::literals; + using namespace std::chrono_literals; + using namespace hal::literals; - bool going_to_60 = false; - for (int i = 0; i < p_homing_structs.size(); i++) { - HAL_CHECK(p_homing_structs[i]->servo->position(0.0_deg)); - hal::delay(p_counter, 2ms); - } + bool going_to_60 = false; + for (int i = 0; i < p_homing_structs.size(); i++) { + HAL_CHECK(p_homing_structs[i]->servo->position(0.0_deg)); + hal::delay(p_counter, 2ms); + } - // max time needed to move to 0 from anywhere at 2 rpms - hal::delay(p_counter, 6s); + // max time needed to move to 0 from anywhere at 2 rpms + hal::delay(p_counter, 6s); - // move them if 0 was home position due to inaccuracy - for (int i = 0; i < p_homing_structs.size(); i++) { - if (!(HAL_CHECK(p_homing_structs[i]->magnet->level()).state)) { - p_homing_structs[i]->servo->set_offset(60.0_deg); - HAL_CHECK(p_homing_structs[i]->servo->position(60.0_deg)); - going_to_60 = true; - } + // move them if 0 was home position due to inaccuracy + for (int i = 0; i < p_homing_structs.size(); i++) { + if (!(HAL_CHECK(p_homing_structs[i]->magnet->level()).state)) { + p_homing_structs[i]->servo->set_offset(60.0_deg); + HAL_CHECK(p_homing_structs[i]->servo->position(60.0_deg)); + going_to_60 = true; } + } - // then if it is going to 60 we need to delay the same amount as above - if (going_to_60) { - hal::delay(p_counter, 6s); - } + // then if it is going to 60 we need to delay the same amount as above + if (going_to_60) { + hal::delay(p_counter, 6s); + } - int number_of_legs_homed = 0; - while (number_of_legs_homed != p_homing_structs.size()) { - for (int i = 0; i < p_homing_structs.size(); i++) { - if(!p_homing_structs[i]->homed) { + int number_of_legs_homed = 0; + while (number_of_legs_homed != p_homing_structs.size()) { + for (int i = 0; i < p_homing_structs.size(); i++) { + if (!p_homing_structs[i]->homed) { - p_homing_structs[i]->homed = !(HAL_CHECK(p_homing_structs[i]->magnet->level()).state); - hal::delay(p_counter, 2ms); - if (p_homing_structs[i]->homed) { - number_of_legs_homed++; - continue; - } - auto new_offset = p_homing_structs[i]->servo->get_offset() + 1.0_deg; - p_homing_structs[i]->servo->set_offset(new_offset); - HAL_CHECK(p_homing_structs[i]->servo->position(0.0_deg)); - } - hal::delay(p_counter, 60ms); + p_homing_structs[i]->homed = + !(HAL_CHECK(p_homing_structs[i]->magnet->level()).state); + hal::delay(p_counter, 2ms); + if (p_homing_structs[i]->homed) { + number_of_legs_homed++; + continue; } + auto new_offset = p_homing_structs[i]->servo->get_offset() + 1.0_deg; + p_homing_structs[i]->servo->set_offset(new_offset); + HAL_CHECK(p_homing_structs[i]->servo->position(0.0_deg)); + } + hal::delay(p_counter, 60ms); } - return hal::success(); + } + return hal::success(); } -} \ No newline at end of file +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/platform-implementations/offset_servo.cpp b/drive/platform-implementations/offset_servo.cpp index 7f5b047..51e2746 100644 --- a/drive/platform-implementations/offset_servo.cpp +++ b/drive/platform-implementations/offset_servo.cpp @@ -1,24 +1,33 @@ #include "../include/offset_servo.hpp" -#include #include +#include namespace sjsu::drive { -hal::result offset_servo::create(hal::servo& p_servo, hal::degrees p_offset) { - return offset_servo(p_servo, p_offset); +hal::result offset_servo::create(hal::servo& p_servo, + hal::degrees p_offset) +{ + return offset_servo(p_servo, p_offset); } -hal::degrees offset_servo::get_offset() { - return m_offset; +hal::degrees offset_servo::get_offset() +{ + return m_offset; } -void offset_servo::set_offset(hal::degrees p_offset) { - m_offset = p_offset; +void offset_servo::set_offset(hal::degrees p_offset) +{ + m_offset = p_offset; } -offset_servo::offset_servo(hal::servo& p_servo, hal::degrees p_offset) : m_servo(&p_servo), m_offset(p_offset) {} - - -hal::result offset_servo::driver_position(hal::degrees p_position) { - return HAL_CHECK(m_servo->position(p_position + m_offset)); +offset_servo::offset_servo(hal::servo& p_servo, hal::degrees p_offset) + : m_servo(&p_servo) + , m_offset(p_offset) +{ } +hal::result offset_servo::driver_position( + hal::degrees p_position) +{ + return HAL_CHECK(m_servo->position(p_position + m_offset)); } + +} // namespace sjsu::drive diff --git a/drive/platform-implementations/serial_mission_control.cpp b/drive/platform-implementations/serial_mission_control.cpp index a0b12bd..f92c31a 100644 --- a/drive/platform-implementations/serial_mission_control.cpp +++ b/drive/platform-implementations/serial_mission_control.cpp @@ -5,11 +5,14 @@ #include "../include/serial_mission_control.hpp" namespace sjsu::drive { - -serial_mission_control::serial_mission_control(hal::serial& p_console) : -m_console(p_console) {} -void serial_mission_control::parse_commands(std::string_view p_commands_json) { +serial_mission_control::serial_mission_control(hal::serial& p_console) + : m_console(p_console) +{ +} + +void serial_mission_control::parse_commands(std::string_view p_commands_json) +{ static constexpr int expected_number_of_arguments = 6; mc_commands commands; @@ -31,7 +34,8 @@ void serial_mission_control::parse_commands(std::string_view p_commands_json) { m_commands = commands; } -hal::result serial_mission_control::impl_get_command( +hal::result +serial_mission_control::impl_get_command( hal::function_ref p_timeout) { std::array buffer{}; @@ -42,7 +46,7 @@ hal::result serial_mission_control::impl_get_comma // hal::print<200>( // m_console, "%.*s", static_cast(response.size()), response.data()); parse_commands(response); - } - return m_commands; } -} // namespace drive \ No newline at end of file + return m_commands; +} +} // namespace sjsu::drive \ No newline at end of file diff --git a/drive/platforms/lpc4078.cpp b/drive/platforms/lpc4078.cpp index a68ebcd..5a253ee 100644 --- a/drive/platforms/lpc4078.cpp +++ b/drive/platforms/lpc4078.cpp @@ -5,39 +5,39 @@ #include #include +#include #include #include #include -#include -#include -#include #include +#include +#include #include "../include/drc_speed_sensor.hpp" -#include "../include/mission_control.hpp" -#include "../include/esp8266_mission_control.hpp" #include "../applications/application.hpp" -#include "../platform-implementations/home.hpp" +#include "../include/esp8266_mission_control.hpp" +#include "../include/mission_control.hpp" #include "../include/offset_servo.hpp" #include "../platform-implementations/helper.hpp" - +#include "../platform-implementations/home.hpp" namespace sjsu::drive { -hal::status initialize_processor(){ - +hal::status initialize_processor() +{ + hal::cortex_m::initialize_data_section(); hal::cortex_m::initialize_floating_point_unit(); return hal::success(); } -hal::result initialize_platform() +hal::result initialize_platform() { using namespace hal::literals; using namespace std::chrono_literals; - + // setting clock HAL_CHECK(hal::lpc40::clock::maximum(12.0_MHz)); auto& clock = hal::lpc40::clock::get(); @@ -46,10 +46,12 @@ hal::result initialize_platform() // Serial static std::array recieve_buffer0{}; - static auto uart0 = HAL_CHECK((hal::lpc40::uart::get(0, recieve_buffer0, hal::serial::settings{ - .baud_rate = 38400, - }))); - // servos, we need to init all of the mc_x motors then call make_servo + static auto uart0 = HAL_CHECK((hal::lpc40::uart::get(0, + recieve_buffer0, + hal::serial::settings{ + .baud_rate = 38400, + }))); + // servos, we need to init all of the mc_x motors then call make_servo // in order to pass servos into the application static hal::can::settings can_settings{ .baud_rate = 1.0_MHz }; static auto can = HAL_CHECK((hal::lpc40::can::get(2, can_settings))); @@ -57,123 +59,157 @@ hal::result initialize_platform() static auto can_router = hal::can_router::create(can).value(); // left leg - static auto left_leg_steer_drc = HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x141)); - static auto left_leg_drc_servo = HAL_CHECK(hal::make_servo(left_leg_steer_drc, 5.0_rpm)); - static auto left_leg_drc_steer_speed_sensor = HAL_CHECK(make_speed_sensor(left_leg_steer_drc)); - auto left_leg_mag = HAL_CHECK(hal::lpc40::input_pin::get(1, 22, hal::input_pin::settings{})); - - static auto left_leg_hub_drc = HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x142)); - static auto left_leg_drc_motor = HAL_CHECK(hal::make_motor(left_leg_hub_drc, 100.0_rpm)); - static auto left_leg_drc_hub_speed_sensor = HAL_CHECK(make_speed_sensor(left_leg_hub_drc)); - - static auto left_leg_drc_offset_servo = HAL_CHECK(offset_servo::create(left_leg_drc_servo, 0.0f)); - - static auto left_home = homing{&left_leg_drc_offset_servo, &left_leg_mag, false}; - // static auto left_leg_drc_speed_sensor = HAL_CHECK(print_speed_sensor::make_speed_sensor(uart0)); - // static auto left_leg_drc_offset_servo = HAL_CHECK(print_servo::create(uart0)); - // static auto left_leg_drc_motor = HAL_CHECK(print_motor::create(uart0)); - + static auto left_leg_steer_drc = + HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x141)); + static auto left_leg_drc_servo = + HAL_CHECK(hal::make_servo(left_leg_steer_drc, 5.0_rpm)); + static auto left_leg_drc_steer_speed_sensor = + HAL_CHECK(make_speed_sensor(left_leg_steer_drc)); + auto left_leg_mag = + HAL_CHECK(hal::lpc40::input_pin::get(1, 22, hal::input_pin::settings{})); + + static auto left_leg_hub_drc = + HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x142)); + static auto left_leg_drc_motor = + HAL_CHECK(hal::make_motor(left_leg_hub_drc, 100.0_rpm)); + static auto left_leg_drc_hub_speed_sensor = + HAL_CHECK(make_speed_sensor(left_leg_hub_drc)); + + static auto left_leg_drc_offset_servo = + HAL_CHECK(offset_servo::create(left_leg_drc_servo, 0.0f)); + + static auto left_home = + homing{ &left_leg_drc_offset_servo, &left_leg_mag, false }; + // static auto left_leg_drc_speed_sensor = + // HAL_CHECK(print_speed_sensor::make_speed_sensor(uart0)); static auto + // left_leg_drc_offset_servo = HAL_CHECK(print_servo::create(uart0)); static + // auto left_leg_drc_motor = HAL_CHECK(print_motor::create(uart0)); // right leg - static auto right_leg_steer_drc = HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x143)); - static auto right_leg_drc_servo = HAL_CHECK(hal::make_servo(right_leg_steer_drc, 5.0_rpm)); - static auto right_leg_drc_steer_speed_sensor = HAL_CHECK(make_speed_sensor(right_leg_steer_drc)); - auto right_leg_mag = HAL_CHECK(hal::lpc40::input_pin::get(1, 15, hal::input_pin::settings{})); - - static auto right_leg_hub_drc = HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x144)); - static auto right_leg_drc_motor = HAL_CHECK(hal::make_motor(right_leg_hub_drc, 100.0_rpm)); - static auto right_leg_drc_hub_speed_sensor = HAL_CHECK(make_speed_sensor(right_leg_hub_drc)); - - static auto right_leg_drc_offset_servo = HAL_CHECK(offset_servo::create(right_leg_drc_servo, 0.0f)); - static auto right_home = homing{&right_leg_drc_offset_servo, &right_leg_mag, false}; - // static auto right_leg_drc_speed_sensor = HAL_CHECK(print_speed_sensor::make_speed_sensor(uart0)); - // static auto right_leg_drc_offset_servo = HAL_CHECK(print_servo::create(uart0)); - // static auto right_leg_drc_motor = HAL_CHECK(print_motor::create(uart0)); - + static auto right_leg_steer_drc = + HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x143)); + static auto right_leg_drc_servo = + HAL_CHECK(hal::make_servo(right_leg_steer_drc, 5.0_rpm)); + static auto right_leg_drc_steer_speed_sensor = + HAL_CHECK(make_speed_sensor(right_leg_steer_drc)); + auto right_leg_mag = + HAL_CHECK(hal::lpc40::input_pin::get(1, 15, hal::input_pin::settings{})); + + static auto right_leg_hub_drc = + HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x144)); + static auto right_leg_drc_motor = + HAL_CHECK(hal::make_motor(right_leg_hub_drc, 100.0_rpm)); + static auto right_leg_drc_hub_speed_sensor = + HAL_CHECK(make_speed_sensor(right_leg_hub_drc)); + + static auto right_leg_drc_offset_servo = + HAL_CHECK(offset_servo::create(right_leg_drc_servo, 0.0f)); + static auto right_home = + homing{ &right_leg_drc_offset_servo, &right_leg_mag, false }; + // static auto right_leg_drc_speed_sensor = + // HAL_CHECK(print_speed_sensor::make_speed_sensor(uart0)); static auto + // right_leg_drc_offset_servo = HAL_CHECK(print_servo::create(uart0)); static + // auto right_leg_drc_motor = HAL_CHECK(print_motor::create(uart0)); // back leg - static auto back_leg_steer_drc = HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x145)); - static auto back_leg_drc_servo = HAL_CHECK(hal::make_servo(back_leg_steer_drc, 5.0_rpm)); - static auto back_leg_drc_steer_speed_sensor = HAL_CHECK(make_speed_sensor(back_leg_steer_drc)); - auto back_leg_mag = HAL_CHECK(hal::lpc40::input_pin::get(1, 23, hal::input_pin::settings{})); - - static auto back_leg_hub_drc = HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x146)); - static auto back_leg_drc_motor = HAL_CHECK(hal::make_motor(back_leg_hub_drc, 100.0_rpm)); - static auto back_leg_drc_hub_speed_sensor = HAL_CHECK(make_speed_sensor(back_leg_hub_drc)); - - static auto back_leg_drc_offset_servo = HAL_CHECK(offset_servo::create(back_leg_drc_servo, 0.0f)); - static auto back_home = homing{&back_leg_drc_offset_servo, &back_leg_mag, false}; - - // static auto back_leg_drc_speed_sensor = HAL_CHECK(print_speed_sensor::make_speed_sensor(uart0)); - // static auto back_leg_drc_offset_servo = HAL_CHECK(print_servo::create(uart0)); - // static auto back_leg_drc_motor = HAL_CHECK(print_motor::create(uart0)); - + static auto back_leg_steer_drc = + HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x145)); + static auto back_leg_drc_servo = + HAL_CHECK(hal::make_servo(back_leg_steer_drc, 5.0_rpm)); + static auto back_leg_drc_steer_speed_sensor = + HAL_CHECK(make_speed_sensor(back_leg_steer_drc)); + auto back_leg_mag = + HAL_CHECK(hal::lpc40::input_pin::get(1, 23, hal::input_pin::settings{})); + + static auto back_leg_hub_drc = + HAL_CHECK(hal::rmd::drc::create(can_router, counter, 6.0, 0x146)); + static auto back_leg_drc_motor = + HAL_CHECK(hal::make_motor(back_leg_hub_drc, 100.0_rpm)); + static auto back_leg_drc_hub_speed_sensor = + HAL_CHECK(make_speed_sensor(back_leg_hub_drc)); + + static auto back_leg_drc_offset_servo = + HAL_CHECK(offset_servo::create(back_leg_drc_servo, 0.0f)); + static auto back_home = + homing{ &back_leg_drc_offset_servo, &back_leg_mag, false }; + + // static auto back_leg_drc_speed_sensor = + // HAL_CHECK(print_speed_sensor::make_speed_sensor(uart0)); static auto + // back_leg_drc_offset_servo = HAL_CHECK(print_servo::create(uart0)); static + // auto back_leg_drc_motor = HAL_CHECK(print_motor::create(uart0)); // extra leg - // static auto extra_leg_steer_drc = HAL_CHECK(hal::rmd::drc::create(can_router, counter, 8.0, 0x147)); - // static auto extra_leg_drc_servo = HAL_CHECK(hal::make_servo(extra_leg_steer_drc, 5.0_rpm)); - // auto extra_leg_mag = HAL_CHECK(hal::lpc40::input_pin::get(1, 23, hal::input_pin::settings{})); - - // static auto extra_leg_hub_drc = HAL_CHECK(hal::rmd::drc::create(can_router, counter, 8.0, 0x148)); - // static auto extra_leg_drc_motor = HAL_CHECK(hal::make_motor(extra_leg_hub_drc, 100.0_rpm)); - // static auto extra_leg_drc_speed_sensor = HAL_CHECK(make_speed_sensor(extra_leg_hub_drc)); - - // static auto extra_leg_drc_offset_servo = HAL_CHECK(offset_servo::create(extra_leg_drc_servo, 0.0f)); - // static auto extra_home = homing{&extra_leg_drc_offset_servo, &extra_leg_mag, false}; + // static auto extra_leg_steer_drc = + // HAL_CHECK(hal::rmd::drc::create(can_router, counter, 8.0, 0x147)); static + // auto extra_leg_drc_servo = + // HAL_CHECK(hal::make_servo(extra_leg_steer_drc, 5.0_rpm)); auto + // extra_leg_mag = HAL_CHECK(hal::lpc40::input_pin::get(1, 23, + // hal::input_pin::settings{})); + + // static auto extra_leg_hub_drc = HAL_CHECK(hal::rmd::drc::create(can_router, + // counter, 8.0, 0x148)); static auto extra_leg_drc_motor = + // HAL_CHECK(hal::make_motor(extra_leg_hub_drc, 100.0_rpm)); static auto + // extra_leg_drc_speed_sensor = + // HAL_CHECK(make_speed_sensor(extra_leg_hub_drc)); + + // static auto extra_leg_drc_offset_servo = + // HAL_CHECK(offset_servo::create(extra_leg_drc_servo, 0.0f)); static auto + // extra_home = homing{&extra_leg_drc_offset_servo, &extra_leg_mag, false}; const size_t number_of_legs = 3; static std::array homing_structs = { - &left_home, - &right_home, - &back_home, - // &extra_home, - }; + &left_home, &right_home, &back_home, + // &extra_home, + }; HAL_CHECK(home(homing_structs, counter)); // hal::print<100>(uart0, "right offset: %f", right_home.servo->get_offset()); // hal::print<100>(uart0, "left offset: %f", left_home.servo->get_offset()); // hal::print<100>(uart0, "back offset: %f", back_home.servo->get_offset()); - static leg left_leg{.steer = &left_leg_drc_offset_servo, - .propulsion = &left_leg_drc_motor, - .steer_speed_sensor = &left_leg_drc_steer_speed_sensor, - .propulsion_speed_sensor = &left_leg_drc_hub_speed_sensor, - }; - - static leg right_leg{.steer = &right_leg_drc_offset_servo, - .propulsion = &right_leg_drc_motor, - .steer_speed_sensor = &right_leg_drc_steer_speed_sensor, - .propulsion_speed_sensor = &right_leg_drc_hub_speed_sensor, - }; - - static leg back_leg{.steer = &back_leg_drc_offset_servo, - .propulsion = &back_leg_drc_motor, - .steer_speed_sensor = &back_leg_drc_steer_speed_sensor, - .propulsion_speed_sensor = &back_leg_drc_hub_speed_sensor - }; - // static leg extra_leg{.steer = &extra_leg_drc_offset_servo, + static leg left_leg{ + .steer = &left_leg_drc_offset_servo, + .propulsion = &left_leg_drc_motor, + .steer_speed_sensor = &left_leg_drc_steer_speed_sensor, + .propulsion_speed_sensor = &left_leg_drc_hub_speed_sensor, + }; + + static leg right_leg{ + .steer = &right_leg_drc_offset_servo, + .propulsion = &right_leg_drc_motor, + .steer_speed_sensor = &right_leg_drc_steer_speed_sensor, + .propulsion_speed_sensor = &right_leg_drc_hub_speed_sensor, + }; + + static leg back_leg{ .steer = &back_leg_drc_offset_servo, + .propulsion = &back_leg_drc_motor, + .steer_speed_sensor = &back_leg_drc_steer_speed_sensor, + .propulsion_speed_sensor = + &back_leg_drc_hub_speed_sensor }; + // static leg extra_leg{.steer = &extra_leg_drc_offset_servo, // .propulsion = &extra_leg_drc_motor, // .spd_sensor = &extra_leg_drc_speed_sensor, // .propulsion_speed_sensor = & // }; -// + // static std::array legs = { - &left_leg, - &right_leg, - &back_leg, + &left_leg, &right_leg, &back_leg, // extra_leg, }; // mission control object static std::array recieve_buffer1{}; - static auto uart1 = - HAL_CHECK((hal::lpc40::uart::get(1, recieve_buffer1, hal::serial::settings{ - .baud_rate = 115200, - }))); + static auto uart1 = HAL_CHECK((hal::lpc40::uart::get(1, + recieve_buffer1, + hal::serial::settings{ + .baud_rate = 115200, + }))); - static constexpr std::string_view ssid = "TP-Link_FC30"; //change to wifi name that you are using - static constexpr std::string_view password = "R0Bot1cs3250"; // change to wifi password you are using + static constexpr std::string_view ssid = + "TP-Link_FC30"; // change to wifi name that you are using + static constexpr std::string_view password = + "R0Bot1cs3250"; // change to wifi password you are using // still need to decide what we want the static IP to be static constexpr std::string_view ip = ""; @@ -184,8 +220,8 @@ hal::result initialize_platform() }; HAL_CHECK(hal::write(uart0, "created Socket\n")); static constexpr std::string_view get_request = "GET /drive HTTP/1.1\r\n" - "Host: 192.168.0.211:5000\r\n" - "\r\n"; + "Host: 192.168.0.211:5000\r\n" + "\r\n"; static std::array buffer{}; // static auto helper = serial_mirror(uart1, uart0); @@ -193,30 +229,35 @@ hal::result initialize_platform() auto timeout = hal::create_timeout(counter, 10s); static auto esp8266 = HAL_CHECK(hal::esp8266::at::create(uart1, timeout)); auto mc_timeout = hal::create_timeout(counter, 10s); - esp8266_mission_control::create_t create_mission_control{.esp8266 = esp8266, - .console = uart0, - .ssid = ssid, - .password = password, - .config = socket_config, - .ip = ip, - .buffer = buffer, - .get_request = get_request}; - static auto esp_mission_control = esp8266_mission_control::create(create_mission_control, mc_timeout); - - while(esp_mission_control.has_error()) { + esp8266_mission_control::create_t create_mission_control{ + .esp8266 = esp8266, + .console = uart0, + .ssid = ssid, + .password = password, + .config = socket_config, + .ip = ip, + .buffer = buffer, + .get_request = get_request + }; + static auto esp_mission_control = + esp8266_mission_control::create(create_mission_control, mc_timeout); + + while (esp_mission_control.has_error()) { mc_timeout = hal::create_timeout(counter, 30s); - esp_mission_control = esp8266_mission_control::create(create_mission_control, mc_timeout); + esp_mission_control = + esp8266_mission_control::create(create_mission_control, mc_timeout); } static auto drive_mission_control = esp_mission_control.value(); - return application_framework{.legs = legs, + return application_framework{ + .legs = legs, - .mc = &drive_mission_control, - - .terminal = &uart0, - .clock = &counter, - .reset = []() { hal::cortex_m::reset(); }, - }; -} + .mc = &drive_mission_control, + .terminal = &uart0, + .clock = &counter, + .reset = []() { hal::cortex_m::reset(); }, + }; } + +} // namespace sjsu::drive From c2a297699f2c09f453a1c48a59342636649704bc Mon Sep 17 00:00:00 2001 From: Coreyboy1820 <92711317+Coreyboy1820@users.noreply.github.com> Date: Wed, 20 Dec 2023 15:20:46 -0800 Subject: [PATCH 3/6] finished deleting unneeded files, formating arm, and creating formating bash script for those whose clang-format doesn't work (#103) --- arm/.clang-format | 9 -- arm/.gitignore | 58 ---------- arm/applications/application.cpp | 21 ++-- arm/applications/application.hpp | 2 +- .../mission_control_handler.hpp | 5 +- arm/implementations/rules_engine.hpp | 5 +- arm/implementations/speed_control.hpp | 104 +++++++++--------- arm/platforms/lpc4078.cpp | 71 +++++++----- 8 files changed, 113 insertions(+), 162 deletions(-) delete mode 100644 arm/.clang-format delete mode 100644 arm/.gitignore diff --git a/arm/.clang-format b/arm/.clang-format deleted file mode 100644 index b72f919..0000000 --- a/arm/.clang-format +++ /dev/null @@ -1,9 +0,0 @@ -BasedOnStyle: Mozilla -Language: Cpp -UseTab: Never -AlwaysBreakAfterReturnType: None -AlwaysBreakAfterDefinitionReturnType: None -AllowShortFunctionsOnASingleLine: None -FixNamespaceComments: true -SpacesBeforeTrailingComments: 2 -ColumnLimit: 80 diff --git a/arm/.gitignore b/arm/.gitignore deleted file mode 100644 index 112c1c6..0000000 --- a/arm/.gitignore +++ /dev/null @@ -1,58 +0,0 @@ -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - -# IDEs -.vscode/ -archives/ - -# GDB -*/**/.gdb_history - -# Artifacts -build/ -docs/api/ -docs/doxygen_warn.log -tools/ - -# lint artifacts -ncc.stderr - -# OS files -.DS_Store - -# Conan Files -graph_info.json -conaninfo.txt -conan.lock -conanbuildinfo.txt -CMakeUserPresets.json diff --git a/arm/applications/application.cpp b/arm/applications/application.cpp index ee38b95..ba8513c 100644 --- a/arm/applications/application.cpp +++ b/arm/applications/application.cpp @@ -1,11 +1,10 @@ #include -#include #include +#include #include "../implementations/joint_router.hpp" -#include "../implementations/speed_control.hpp" #include "../implementations/rules_engine.hpp" - +#include "../implementations/speed_control.hpp" #include "application.hpp" @@ -29,14 +28,14 @@ hal::status application(sjsu::arm::application_framework& p_framework) // auto& end_effector = *p_framework.end_effector; // mission control init should go here, if anything is needed - + std::string_view json; joint_router arm(rotunda_servo, - shoulder_servo, - elbow_servo, - left_wrist_servo, - right_wrist_servo); + shoulder_servo, + elbow_servo, + left_wrist_servo, + right_wrist_servo); sjsu::arm::mission_control::mc_commands commands; speed_control speed_control; @@ -45,10 +44,10 @@ hal::status application(sjsu::arm::application_framework& p_framework) hal::delay(clock, 1000ms); while (true) { - if(loop_count==10) { + if (loop_count == 10) { auto timeout = hal::create_timeout(clock, 60ms); commands = mission_control.get_command(timeout).value(); - loop_count=0; + loop_count = 0; } loop_count++; commands = validate_commands(commands); @@ -61,4 +60,4 @@ hal::status application(sjsu::arm::application_framework& p_framework) return hal::success(); } -} +} // namespace sjsu::arm diff --git a/arm/applications/application.hpp b/arm/applications/application.hpp index 561a99c..1b28c1b 100644 --- a/arm/applications/application.hpp +++ b/arm/applications/application.hpp @@ -28,4 +28,4 @@ hal::status initialize_processor(); hal::result initialize_platform(); hal::status application(application_framework& p_framework); -} +} // namespace sjsu::arm diff --git a/arm/implementations/mission_control_handler.hpp b/arm/implementations/mission_control_handler.hpp index ede1bc5..37a24d4 100644 --- a/arm/implementations/mission_control_handler.hpp +++ b/arm/implementations/mission_control_handler.hpp @@ -4,9 +4,8 @@ #include "../dto/feedback_dto.hpp" #include "../implementations/mission_control_handler.hpp" -inline hal::status parse_mission_control_data( - std::string response, - hal::serial& terminal) +inline hal::status parse_mission_control_data(std::string response, + hal::serial& terminal) { // static constexpr int expected_number_of_arguments = 9; // sjsu::arm::mission_control::mc_commands commands; diff --git a/arm/implementations/rules_engine.hpp b/arm/implementations/rules_engine.hpp index fa595ae..fc75da8 100644 --- a/arm/implementations/rules_engine.hpp +++ b/arm/implementations/rules_engine.hpp @@ -10,7 +10,8 @@ namespace sjsu::arm { // static constexpr int kMinWristPitchAngle = -90; // static constexpr int kMaxWristPitchAngle = 90; -inline mission_control::mc_commands validate_commands(mission_control::mc_commands commands) +inline mission_control::mc_commands validate_commands( + mission_control::mc_commands commands) { if (commands.speed < 1 || commands.speed > 5) { @@ -22,4 +23,4 @@ inline mission_control::mc_commands validate_commands(mission_control::mc_comman // } return commands; } -} // namespace arm \ No newline at end of file +} // namespace sjsu::arm \ No newline at end of file diff --git a/arm/implementations/speed_control.hpp b/arm/implementations/speed_control.hpp index c2fc36c..159259c 100644 --- a/arm/implementations/speed_control.hpp +++ b/arm/implementations/speed_control.hpp @@ -4,56 +4,58 @@ namespace sjsu::arm { -class speed_control { - public: - - mission_control::mc_commands lerp(mission_control::mc_commands p_commands_to_set) { - - float lerp_constant = .01f; - - // lerping rotunda - p_commands_to_set.rotunda_angle = - std::lerp(m_current_settings.rotunda_angle, - p_commands_to_set.rotunda_angle, - p_commands_to_set.speed * lerp_constant); - - // lerping shoulder - p_commands_to_set.shoulder_angle = - std::lerp(m_current_settings.shoulder_angle, - p_commands_to_set.shoulder_angle, - p_commands_to_set.speed * lerp_constant); - - // lerping elbow - p_commands_to_set.elbow_angle = - std::lerp(m_current_settings.elbow_angle, - p_commands_to_set.elbow_angle, - p_commands_to_set.speed * lerp_constant); - - // lerping wrist pitch - p_commands_to_set.wrist_pitch_angle = - std::lerp(m_current_settings.wrist_pitch_angle, - p_commands_to_set.wrist_pitch_angle, - p_commands_to_set.speed * lerp_constant); - - // lerping wrist roll - p_commands_to_set.wrist_roll_angle = - std::lerp(m_current_settings.wrist_roll_angle, - p_commands_to_set.wrist_roll_angle, - p_commands_to_set.speed * lerp_constant); - - // lerping end effector - // p_commands_to_set.end_effector_angle = - // std::lerp(m_current_settings.end_effector_angle, - // p_commands_to_set.end_effector_angle, - // p_commands_to_set.speed * lerp_constant); - - // set this to the current value to allow for lerping to the next step - m_current_settings = p_commands_to_set; - return p_commands_to_set; - } - - private: - mission_control::mc_commands m_current_settings; +class speed_control +{ +public: + mission_control::mc_commands lerp( + mission_control::mc_commands p_commands_to_set) + { + + float lerp_constant = .01f; + + // lerping rotunda + p_commands_to_set.rotunda_angle = + std::lerp(m_current_settings.rotunda_angle, + p_commands_to_set.rotunda_angle, + p_commands_to_set.speed * lerp_constant); + + // lerping shoulder + p_commands_to_set.shoulder_angle = + std::lerp(m_current_settings.shoulder_angle, + p_commands_to_set.shoulder_angle, + p_commands_to_set.speed * lerp_constant); + + // lerping elbow + p_commands_to_set.elbow_angle = + std::lerp(m_current_settings.elbow_angle, + p_commands_to_set.elbow_angle, + p_commands_to_set.speed * lerp_constant); + + // lerping wrist pitch + p_commands_to_set.wrist_pitch_angle = + std::lerp(m_current_settings.wrist_pitch_angle, + p_commands_to_set.wrist_pitch_angle, + p_commands_to_set.speed * lerp_constant); + + // lerping wrist roll + p_commands_to_set.wrist_roll_angle = + std::lerp(m_current_settings.wrist_roll_angle, + p_commands_to_set.wrist_roll_angle, + p_commands_to_set.speed * lerp_constant); + + // lerping end effector + // p_commands_to_set.end_effector_angle = + // std::lerp(m_current_settings.end_effector_angle, + // p_commands_to_set.end_effector_angle, + // p_commands_to_set.speed * lerp_constant); + + // set this to the current value to allow for lerping to the next step + m_current_settings = p_commands_to_set; + return p_commands_to_set; + } + +private: + mission_control::mc_commands m_current_settings; }; -} \ No newline at end of file +} // namespace sjsu::arm \ No newline at end of file diff --git a/arm/platforms/lpc4078.cpp b/arm/platforms/lpc4078.cpp index dcb76fc..f9a77e3 100644 --- a/arm/platforms/lpc4078.cpp +++ b/arm/platforms/lpc4078.cpp @@ -11,8 +11,8 @@ #include // #include #include "../applications/application.hpp" -#include "../platform-implementations/helper.hpp" #include "../platform-implementations/esp8266_mission_control.cpp" +#include "../platform-implementations/helper.hpp" #include #include @@ -59,13 +59,13 @@ hal::result initialize_platform() static auto rotunda_mc_x_servo = HAL_CHECK(hal::make_servo(rotunda_mc_x, 2.0_rpm)); - static auto shoulder_mc_x = - HAL_CHECK(hal::rmd::mc_x::create(can_router, counter, 36.0 * 65 / 30, 0x142)); + static auto shoulder_mc_x = HAL_CHECK( + hal::rmd::mc_x::create(can_router, counter, 36.0 * 65 / 30, 0x142)); static auto shoulder_mc_x_servo = HAL_CHECK(hal::make_servo(shoulder_mc_x, 2.0_rpm)); - static auto elbow_mc_x = - HAL_CHECK(hal::rmd::mc_x::create(can_router, counter, 36.0 * 40 / 30, 0x143)); + static auto elbow_mc_x = HAL_CHECK( + hal::rmd::mc_x::create(can_router, counter, 36.0 * 40 / 30, 0x143)); static auto elbow_mc_x_servo = HAL_CHECK(hal::make_servo(elbow_mc_x, 2.0_rpm)); @@ -79,9 +79,9 @@ hal::result initialize_platform() static auto right_wrist_mc_x_servo = HAL_CHECK(hal::make_servo(right_wrist_mc_x, 2.0_rpm)); - // static auto pca9685 = HAL_CHECK(hal::pca::pca9685::create(i2c, 0b100'0000)); - // static auto& pwm0 = pca9685.get_pwm_channel<0>(); - // auto servo_settings = hal::rc_servo::settings{ + // static auto pca9685 = HAL_CHECK(hal::pca::pca9685::create(i2c, + // 0b100'0000)); static auto& pwm0 = pca9685.get_pwm_channel<0>(); auto + // servo_settings = hal::rc_servo::settings{ // .min_angle = 0.0_deg, // .max_angle = 180.0_deg, // .min_microseconds = 500, @@ -90,17 +90,20 @@ hal::result initialize_platform() // static auto& end_effector_servo = // HAL_CHECK(hal::rc_servo::create(pwm0, servo_settings)) - // mission control object -// mission control object + // mission control object + // mission control object static std::array recieve_buffer1{}; - static auto uart1 = - HAL_CHECK((hal::lpc40::uart::get(1, recieve_buffer1, hal::serial::settings{ - .baud_rate = 115200, - }))); + static auto uart1 = HAL_CHECK((hal::lpc40::uart::get(1, + recieve_buffer1, + hal::serial::settings{ + .baud_rate = 115200, + }))); - static constexpr std::string_view ssid = "TP-Link_FC30"; //change to wifi name that you are using - static constexpr std::string_view password = "R0Bot1cs3250"; // change to wifi password you are using + static constexpr std::string_view ssid = + "TP-Link_FC30"; // change to wifi name that you are using + static constexpr std::string_view password = + "R0Bot1cs3250"; // change to wifi password you are using // still need to decide what we want the static IP to be static constexpr std::string_view ip = "192.168.0.212"; @@ -111,10 +114,10 @@ hal::result initialize_platform() }; HAL_CHECK(hal::write(uart0, "created Socket\n")); static constexpr std::string_view get_request = "GET /arm HTTP/1.1\r\n" - "Host: 192.168.0.211:5000\r\n" - "Keep-Alive: timeout=1000\r\n" - "Connection: keep-alive\r\n" - "\r\n"; + "Host: 192.168.0.211:5000\r\n" + "Keep-Alive: timeout=1000\r\n" + "Connection: keep-alive\r\n" + "\r\n"; static std::array buffer{}; static auto helper = serial_mirror(uart1, uart0); @@ -122,14 +125,28 @@ hal::result initialize_platform() auto timeout = hal::create_timeout(counter, 10s); static auto esp8266 = HAL_CHECK(hal::esp8266::at::create(helper, timeout)); auto mc_timeout = hal::create_timeout(counter, 10s); - static auto esp_mission_control = sjsu::arm::esp8266_mission_control::create(esp8266, - uart0, ssid, password, socket_config, - ip, mc_timeout, buffer, get_request); - while(esp_mission_control.has_error()) { + static auto esp_mission_control = + sjsu::arm::esp8266_mission_control::create(esp8266, + uart0, + ssid, + password, + socket_config, + ip, + mc_timeout, + buffer, + get_request); + while (esp_mission_control.has_error()) { mc_timeout = hal::create_timeout(counter, 10s); - esp_mission_control = sjsu::arm::esp8266_mission_control::create(esp8266, - uart0, ssid, password, socket_config, - ip, mc_timeout, buffer, get_request); + esp_mission_control = + sjsu::arm::esp8266_mission_control::create(esp8266, + uart0, + ssid, + password, + socket_config, + ip, + mc_timeout, + buffer, + get_request); } static auto arm_mission_control = esp_mission_control.value(); From 1ccbf9873331589b3e77184ed5e5e104a3ee5020 Mon Sep 17 00:00:00 2001 From: Nina Wang <115374853+ninaw04@users.noreply.github.com> Date: Fri, 12 Jan 2024 16:14:32 -0800 Subject: [PATCH 4/6] =?UTF-8?q?=E2=9C=A8=20Port=20arm=20mimic=20(#105)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * :boom: arm mimic working and testing --------- Co-authored-by: Coreyboy1820 <92711317+Coreyboy1820@users.noreply.github.com> Co-authored-by: corey Co-authored-by: Viha123 Co-authored-by: FluffyFTW Co-authored-by: vihashah Co-authored-by: Andrew Lin --- arm/.clang-format | 9 + arm/.gitignore | 58 +++ arm/implementations/speed_control.hpp | 2 +- arm_mimic/CMakeLists.txt | 42 ++ arm_mimic/applications/application.cpp | 60 +++ arm_mimic/applications/application.hpp | 20 + arm_mimic/conanfile.py | 41 ++ .../implementations/degree_manipulation.hpp | 12 + .../mission_control_handler.hpp | 31 ++ arm_mimic/implementations/tla2528.hpp | 132 ++++++ arm_mimic/main.cpp | 114 +++++ arm_mimic/platforms/lpc4078.cpp | 56 +++ drive/dto/motor_feedback.hpp | 2 +- drive/implementations/rules_engine.hpp | 2 +- drive/implementations/tri_wheel_router.hpp | 64 +++ .../drc_speed_sensor.cpp | 2 +- .../drc_speed_sensor.hpp | 19 + .../esp8266_mission_control.cpp | 2 +- drive/platform-implementations/home.hpp | 2 +- .../mission_control.hpp | 83 ++++ .../platform-implementations/offset_servo.hpp | 35 ++ .../print_mission_control.hpp | 41 ++ .../platform-implementations/print_motor.hpp | 28 ++ .../platform-implementations/print_servo.hpp | 29 ++ .../print_speed_sensor.hpp | 34 ++ .../platform-implementations/speed_sensor.hpp | 24 + science/applications/application.cpp | 21 + science/applications/application.hpp | 50 ++ science/demos/CMakeLists.txt | 34 ++ .../applications/pressure_sensor_demo.cpp | 121 +++++ science/demos/conanfile.py | 41 ++ science/demos/hardware_map.hpp | 50 ++ science/demos/main.cpp | 42 ++ science/demos/platforms/lpc4078.cpp | 104 +++++ .../platform-implementations/co2_sensor.hpp | 1 + .../esp8266_mission_control.cpp | 317 +++++++++++++ .../mission_control.hpp | 83 ++++ .../pressure_sensor.hpp | 1 + .../pressure_sensor_bme680.cpp | 428 ++++++++++++++++++ .../pressure_sensor_bme680.hpp | 220 +++++++++ 40 files changed, 2451 insertions(+), 6 deletions(-) create mode 100644 arm/.clang-format create mode 100644 arm/.gitignore create mode 100644 arm_mimic/CMakeLists.txt create mode 100644 arm_mimic/applications/application.cpp create mode 100644 arm_mimic/applications/application.hpp create mode 100644 arm_mimic/conanfile.py create mode 100644 arm_mimic/implementations/degree_manipulation.hpp create mode 100644 arm_mimic/implementations/mission_control_handler.hpp create mode 100644 arm_mimic/implementations/tla2528.hpp create mode 100644 arm_mimic/main.cpp create mode 100644 arm_mimic/platforms/lpc4078.cpp create mode 100644 drive/implementations/tri_wheel_router.hpp create mode 100644 drive/platform-implementations/drc_speed_sensor.hpp create mode 100644 drive/platform-implementations/mission_control.hpp create mode 100644 drive/platform-implementations/offset_servo.hpp create mode 100644 drive/platform-implementations/print_mission_control.hpp create mode 100644 drive/platform-implementations/print_motor.hpp create mode 100644 drive/platform-implementations/print_servo.hpp create mode 100644 drive/platform-implementations/print_speed_sensor.hpp create mode 100644 drive/platform-implementations/speed_sensor.hpp create mode 100644 science/applications/application.cpp create mode 100644 science/applications/application.hpp create mode 100644 science/demos/CMakeLists.txt create mode 100644 science/demos/applications/pressure_sensor_demo.cpp create mode 100644 science/demos/conanfile.py create mode 100644 science/demos/hardware_map.hpp create mode 100644 science/demos/main.cpp create mode 100644 science/demos/platforms/lpc4078.cpp create mode 100644 science/platform-implementations/co2_sensor.hpp create mode 100644 science/platform-implementations/esp8266_mission_control.cpp create mode 100644 science/platform-implementations/mission_control.hpp create mode 100644 science/platform-implementations/pressure_sensor.hpp create mode 100644 science/platform-implementations/pressure_sensor_bme680.cpp create mode 100644 science/platform-implementations/pressure_sensor_bme680.hpp diff --git a/arm/.clang-format b/arm/.clang-format new file mode 100644 index 0000000..b72f919 --- /dev/null +++ b/arm/.clang-format @@ -0,0 +1,9 @@ +BasedOnStyle: Mozilla +Language: Cpp +UseTab: Never +AlwaysBreakAfterReturnType: None +AlwaysBreakAfterDefinitionReturnType: None +AllowShortFunctionsOnASingleLine: None +FixNamespaceComments: true +SpacesBeforeTrailingComments: 2 +ColumnLimit: 80 diff --git a/arm/.gitignore b/arm/.gitignore new file mode 100644 index 0000000..112c1c6 --- /dev/null +++ b/arm/.gitignore @@ -0,0 +1,58 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +# IDEs +.vscode/ +archives/ + +# GDB +*/**/.gdb_history + +# Artifacts +build/ +docs/api/ +docs/doxygen_warn.log +tools/ + +# lint artifacts +ncc.stderr + +# OS files +.DS_Store + +# Conan Files +graph_info.json +conaninfo.txt +conan.lock +conanbuildinfo.txt +CMakeUserPresets.json diff --git a/arm/implementations/speed_control.hpp b/arm/implementations/speed_control.hpp index 159259c..e291253 100644 --- a/arm/implementations/speed_control.hpp +++ b/arm/implementations/speed_control.hpp @@ -58,4 +58,4 @@ class speed_control mission_control::mc_commands m_current_settings; }; -} // namespace sjsu::arm \ No newline at end of file +} // namespace sjsu::arm diff --git a/arm_mimic/CMakeLists.txt b/arm_mimic/CMakeLists.txt new file mode 100644 index 0000000..20d9029 --- /dev/null +++ b/arm_mimic/CMakeLists.txt @@ -0,0 +1,42 @@ +# Copyright 2023 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.20) + +project(starter_project.elf VERSION 0.0.1 LANGUAGES CXX) + +set(platform_library $ENV{LIBHAL_PLATFORM_LIBRARY}) +set(platform $ENV{LIBHAL_PLATFORM}) + +if("${platform_library}" STREQUAL "") + message(FATAL_ERROR + "Build environment variable LIBHAL_PLATFORM_LIBRARY is required for " "this project.") +endif() + +find_package(libhal-${platform_library} REQUIRED CONFIG) +find_package(libhal-util REQUIRED CONFIG) + +add_executable(${PROJECT_NAME} + main.cpp + applications/application.cpp + platforms/${platform}.cpp) +target_include_directories(${PROJECT_NAME} PUBLIC .) +target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_20) +# target_link_options(${PROJECT_NAME} PRIVATE +# --oslib=semihost --crt0=minimal) +target_link_libraries(${PROJECT_NAME} PRIVATE + libhal::${platform_library} + libhal::util) + +libhal_post_build(${PROJECT_NAME}) diff --git a/arm_mimic/applications/application.cpp b/arm_mimic/applications/application.cpp new file mode 100644 index 0000000..5329c71 --- /dev/null +++ b/arm_mimic/applications/application.cpp @@ -0,0 +1,60 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../implementations/tla2528.hpp" +#include "../implementations/degree_manipulation.hpp" +#include "../implementations/mission_control_handler.hpp" + +#include "application.hpp" +namespace sjsu::arm_mimic { + +hal::status application(application_framework& p_framework) +{ + using namespace hal::literals; + + auto& steady_clock = *p_framework.steady_clock; + auto& uart0 = *p_framework.terminal; + auto& i2c2 = *p_framework.i2c2; + auto TLA2528 = tla::tla2528(i2c2, steady_clock); + + const uint8_t N = 6; + std::array scrambled_results = {}; + std::array results = {}; + float true_degree; + float max_volt = 4096; + float max_deg = 360; + + while(true){ + // auto results = HAL_CHECK(TLA2528.read_one(channel)); + // hal::print<64>(uart0, "voltage: %d\n", results); + + auto buffer_of_results = HAL_CHECK(TLA2528.read_all()); + for(int i = 0; i < N; i++) { + true_degree = tla::voltage_to_degree(buffer_of_results[i], max_volt,max_deg); + scrambled_results[i] = true_degree; + // hal::print<64>(uart0, "voltage %d: %d ", i, (int)true_degree); + } + // hal::print<64>(uart0, "\n"); + + results[0] = 0; // ROTUNDA + results[1] = scrambled_results[2] - 261; // SHOULDER + results[2] = -(scrambled_results[1] - 151); // ELBOW + // results[3] = scrambled_results[0]; // WRIST PITCH + // results[4] = scrambled_results[5]; // WRIST ROLL + results[3] = 0; + results[4] = 0; + results[5] = 0; + + HAL_CHECK(tla::send_data_to_mc(*p_framework.terminal, results)); + } +} +} \ No newline at end of file diff --git a/arm_mimic/applications/application.hpp b/arm_mimic/applications/application.hpp new file mode 100644 index 0000000..1b6080f --- /dev/null +++ b/arm_mimic/applications/application.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include +#include +#include +#include + +namespace sjsu::arm_mimic { + struct application_framework + { + hal::serial* terminal; + hal::i2c* i2c2; + hal::steady_clock* steady_clock; + hal::callback reset; + }; + + hal::status initialize_processor(); + hal::result initialize_platform(); + hal::status application(application_framework& p_framework); +} // namespace sjsu::arm_mimic \ No newline at end of file diff --git a/arm_mimic/conanfile.py b/arm_mimic/conanfile.py new file mode 100644 index 0000000..e8246b8 --- /dev/null +++ b/arm_mimic/conanfile.py @@ -0,0 +1,41 @@ +# Copyright 2023 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from conan import ConanFile +from conan.tools.cmake import CMake, cmake_layout + + +class demos(ConanFile): + settings = "compiler", "build_type", "os", "arch" + generators = "CMakeToolchain", "CMakeDeps", "VirtualBuildEnv" + options = {"platform": ["ANY"]} + default_options = {"platform": "unspecified"} + + def build_requirements(self): + self.tool_requires("cmake/3.27.1") + self.tool_requires("libhal-cmake-util/2.2.0") + + def requirements(self): + self.requires("libhal/[^2.0.3]") + if str(self.options.platform).startswith("lpc40"): + self.requires("libhal-lpc40/[^2.1.4]") + + def layout(self): + platform_directory = "build/" + str(self.options.platform) + cmake_layout(self, build_folder=platform_directory) + + def build(self): + cmake = CMake(self) + cmake.configure() + cmake.build() \ No newline at end of file diff --git a/arm_mimic/implementations/degree_manipulation.hpp b/arm_mimic/implementations/degree_manipulation.hpp new file mode 100644 index 0000000..469cc12 --- /dev/null +++ b/arm_mimic/implementations/degree_manipulation.hpp @@ -0,0 +1,12 @@ +#pragma once +#include + +namespace tla { + + float voltage_to_degree(float value, float max_voltage, float max_degree) + { + return hal::map(value, + std::pair(0.0, max_voltage), + std::pair(0.0, max_degree)); + } +} \ No newline at end of file diff --git a/arm_mimic/implementations/mission_control_handler.hpp b/arm_mimic/implementations/mission_control_handler.hpp new file mode 100644 index 0000000..0f580df --- /dev/null +++ b/arm_mimic/implementations/mission_control_handler.hpp @@ -0,0 +1,31 @@ +#pragma once +#include +#include +#include + +namespace tla { + + hal::status send_data_to_mc(hal::serial &uart, std::array raw_data, bool is_network = false) + { + const char *json_format_str = "{\"heartbeat_count\":0,\"is_operational\":1,\"speed\":2,\"angles\":[%d,%d,%d,%d,%d,%d]}\n"; + char buffer[200]; + snprintf(buffer, 200, json_format_str, + static_cast(raw_data[0]), static_cast(raw_data[1]), static_cast(raw_data[2]), static_cast(raw_data[3]), static_cast(raw_data[4]), static_cast(raw_data[5])); + if (!is_network) + hal::print<200>(uart, buffer); + else + return hal::new_error("Not implemented"); + + return hal::success(); + } + +} // namespace arm_mimic + // Structure of json file + /* + { + heartbeat_count: 0, + is_operational: 1, + speed: 2, + angles: [$ROTUNDA, $SHOULDER, $ELBOW, $WRIST_PITCH, $WRIST_ROLL, $END_EFFECTOR] + } + */ \ No newline at end of file diff --git a/arm_mimic/implementations/tla2528.hpp b/arm_mimic/implementations/tla2528.hpp new file mode 100644 index 0000000..ea8f673 --- /dev/null +++ b/arm_mimic/implementations/tla2528.hpp @@ -0,0 +1,132 @@ +#pragma once +#include +#include +#include +#include +#include + +// debugging purposes + +namespace tla { +class tla2528 +{ +public: + tla2528(hal::i2c& p_i2c, hal::steady_clock& p_clk) + : m_bus(p_i2c) + , m_clk{ p_clk } + { + } + + hal::result read_one(hal::byte channel) + { + using namespace std::literals; + + if (channel > 7) { + return hal::new_error( + "Invalid channel\n"); // TODO: switch to an error number, + // strings take up too much space + } + std::array data_buffer; + std::array selection_cmd_buffer = { + OpCodes::SingleRegisterWrite, // Command to write data to a register + RegisterAddresses::CHANNEL_SEL, // Register to select channel + channel + }; + std::array read_cmd_buffer = { OpCodes::SingleRegisterRead }; + + HAL_CHECK(hal::write(m_bus, m_mux_i2c_id, selection_cmd_buffer)); + HAL_CHECK(hal::write(m_bus, m_mux_i2c_id, read_cmd_buffer)); + HAL_CHECK(hal::read(m_bus, m_mux_i2c_id, data_buffer)); + hal::delay(m_clk, 1ms); + + uint16_t data = (data_buffer[0] << 4) | (data_buffer[1] >> 4); + + // return voltage_to_degree(data, 4096, 360); + + return data; + // std::pair p_input_range; + // std::pair p_output_range; + + // p_input_range = std::make_pair(0.0, 4000.0); + // p_output_range = std::make_pair(0,0, 360.0); + + // auto mapped_data = constexpr float hal::map ( + // data, // modifies data directly? + // p_input_range /*something something gotta check*/ , + // p_output_range /*potentiometer range*/ + // ) + // return mapped_data; + }; + + // TODO: Improve code, more testing + hal::result> read_all() + { + std::array result = {}; + for (int i = 0; i < 8; i++) { + result[i] = HAL_CHECK(read_one(i)); + } + + return result; + } + +private: + enum OpCodes + { + // Requests to read data from the mux. (See Figure 29 on datasheet) + SingleRegisterRead = 0b0001'0000, + + // Writes data to a register given an address. (See Figure 31 on data + // sheet) + SingleRegisterWrite = 0b0000'1000, + + // Sets bits in a given register, requires addr + SetBit = 0b0001'1000, + + // Clears bits in a given register + ClearBit = 0b0010'0000, + + // Continuously reads data from a group of registers. Provide the addr of + // the first to read from, reads happen on clk, if it runs out of valid + // addresses to read, it returns zeros. + // Master ACKs to confirm data has + // gotten through. (See Figure 30 on datasheet) + ContinuousRegisterRead = 0b0011'0000, + + // Continuously writes data to a group of registers. Provide the first + // address to write to. Then, send data as bytes slave will automatically + // write the data to the next register in accending order, data is + // seperated by ACKs (slave ACKs). (See Figure 32 on datasheet) + ContinuousRegisterWrite = 0b0010'1000 + }; + + /** + * @brief Most of these are just for configurations, if you need to change + them, see bit layout (Section 7.6 on datasheet) And use SetBit and ClearBit + appropriately to set and clear the bits dictated by each setting. (See + Table 8 on data sheet) + * + */ + enum RegisterAddresses + { + SYSTEM_STATUS = 0x0, + GENERAL_CFG = 0x1, + DATA_CFG = 0x2, + OSR_CFG = 0x3, + OPMODE_CFG = 0x4, + PIN_CFG = 0x5, + GPIO_CFG = 0x7, + GPO_DRIVE_CFG = 0x9, + GPO_VALUE = 0xB, + GPI_VALUE = 0xD, + SEQUENCE_CFG = 0x10, + + // Channel selection register, write the channel number 0-7 that you are + // trying to read from during the write frame. + CHANNEL_SEL = 0x11, + AUTO_SEQ_CH_SEL = 0x12 + }; + const hal::byte m_mux_i2c_id = 0x17; + hal::i2c& m_bus; + hal::steady_clock& m_clk; +}; +} // namespace tla diff --git a/arm_mimic/main.cpp b/arm_mimic/main.cpp new file mode 100644 index 0000000..4d549c2 --- /dev/null +++ b/arm_mimic/main.cpp @@ -0,0 +1,114 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include + +#include "applications/application.hpp" + +int main() +{ + // Step 1. Call the processor initializer. Setups processor to run + // applications such as turning on a coprocessor, or copying memory from + // storage to memory. + if (!sjsu::arm_mimic::initialize_processor()) { + hal::halt(); + } + + auto application_resource = sjsu::arm_mimic::initialize_platform(); + + if (!application_resource) { + hal::halt(); + } + + auto is_finished = sjsu::arm_mimic::application(application_resource.value()); + // do error handling here + + if (!is_finished) { + application_resource.value().reset(); + } else { + hal::halt(); + } + + return 0; +} + +namespace boost { +void throw_exception([[maybe_unused]] std::exception const& p_error) +{ + std::abort(); +} +} // namespace boost + +extern "C" +{ + /// Dummy implementation of getpid + int _getpid_r() + { + return 1; + } + + /// Dummy implementation of kill + int _kill_r(int, int) + { + return -1; + } + + /// Dummy implementation of fstat, makes the assumption that the "device" + /// representing, in this case STDIN, STDOUT, and STDERR as character devices. + int _fstat_r([[maybe_unused]] int file, struct stat* status) + { + status->st_mode = S_IFCHR; + return 0; + } + + int _write_r([[maybe_unused]] int file, + [[maybe_unused]] const char* ptr, + int length) + { + return length; + } + + int _read_r([[maybe_unused]] FILE* file, + [[maybe_unused]] char* ptr, + int length) + { + return length; + } + + // Dummy implementation of _lseek + int _lseek_r([[maybe_unused]] int file, + [[maybe_unused]] int ptr, + [[maybe_unused]] int dir) + { + return 0; + } + + // Dummy implementation of close + int _close_r([[maybe_unused]] int file) + { + return -1; + } + + // Dummy implementation of isatty + int _isatty_r([[maybe_unused]] int file) + { + return 1; + } +} diff --git a/arm_mimic/platforms/lpc4078.cpp b/arm_mimic/platforms/lpc4078.cpp new file mode 100644 index 0000000..646542f --- /dev/null +++ b/arm_mimic/platforms/lpc4078.cpp @@ -0,0 +1,56 @@ +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include "applications/application.hpp" + +namespace sjsu::arm_mimic { + +hal::status initialize_processor(){ + + hal::cortex_m::initialize_data_section(); + hal::cortex_m::initialize_floating_point_unit(); + return hal::success(); +} + +hal::result initialize_platform() +{ + using namespace hal::literals; + using namespace std::chrono_literals; + + auto& clock = hal::lpc40::clock::get(); + auto cpu_frequency = clock.get_frequency(hal::lpc40::peripheral::cpu); + static hal::cortex_m::dwt_counter counter(cpu_frequency); + + // Get and initialize UART0 for UART based terminal logging + // auto& uart0 = HAL_CHECK((hal::lpc40::uart::get<0, 64>(hal::serial::settings{ + // .baud_rate = 38400, + // }))); + static std::array recieve_buffer0{}; + static auto uart0 = HAL_CHECK((hal::lpc40::uart::get(0, + recieve_buffer0, + hal::serial::settings{ + .baud_rate = 38400, + }))); + + static auto i2c2 = HAL_CHECK(hal::lpc40::i2c::get(2)); + + return application_framework{ + .terminal = &uart0, + .i2c2 = &i2c2, + .steady_clock = &counter, + .reset = []() { hal::cortex_m::reset(); }, + }; +}; +} diff --git a/drive/dto/motor_feedback.hpp b/drive/dto/motor_feedback.hpp index 9dd3b46..6fbf0c9 100644 --- a/drive/dto/motor_feedback.hpp +++ b/drive/dto/motor_feedback.hpp @@ -29,4 +29,4 @@ struct motor_feedback } }; -} // namespace sjsu::drive \ No newline at end of file +} // namespace sjsu::drive diff --git a/drive/implementations/rules_engine.hpp b/drive/implementations/rules_engine.hpp index 6f9ef31..87a5821 100644 --- a/drive/implementations/rules_engine.hpp +++ b/drive/implementations/rules_engine.hpp @@ -23,4 +23,4 @@ inline mission_control::mc_commands validate_commands( return p_commands; } -} // namespace sjsu::drive \ No newline at end of file +} // namespace sjsu::drive diff --git a/drive/implementations/tri_wheel_router.hpp b/drive/implementations/tri_wheel_router.hpp new file mode 100644 index 0000000..5250a10 --- /dev/null +++ b/drive/implementations/tri_wheel_router.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "../dto/drive.hpp" +#include "../dto/motor_feedback.hpp" +#include "../applications/application.hpp" + +namespace sjsu::drive { +class tri_wheel_router +{ +public: + + tri_wheel_router(leg& p_back, leg& p_right, leg& p_left) + : m_left(p_left) + , m_back(p_back) + , m_right(p_right) + { + } + + hal::status move(tri_wheel_router_arguments p_tri_wheel_arguments, + hal::steady_clock& p_clock) + { + HAL_CHECK(m_left.steer->position(-p_tri_wheel_arguments.left.angle)); + HAL_CHECK(m_left.propulsion->power(p_tri_wheel_arguments.left.speed/100)); + + HAL_CHECK(m_right.steer->position(-p_tri_wheel_arguments.right.angle)); + HAL_CHECK(m_right.propulsion->power(-p_tri_wheel_arguments.right.speed/100)); + + HAL_CHECK(m_back.steer->position(-p_tri_wheel_arguments.back.angle)); + HAL_CHECK(m_back.propulsion->power(p_tri_wheel_arguments.back.speed/100)); + + return hal::success(); + } + + hal::result get_motor_feedback() + { + using namespace std::chrono_literals; + using namespace hal::literals; + motor_feedback motor_speeds; + + motor_speeds.left_drive_speed = HAL_CHECK(m_left.propulsion_speed_sensor->read()).speed; + motor_speeds.left_steer_speed = HAL_CHECK(m_left.steer_speed_sensor->read()).speed; + motor_speeds.back_drive_speed = HAL_CHECK(m_back.propulsion_speed_sensor->read()).speed; + motor_speeds.back_steer_speed = HAL_CHECK(m_back.steer_speed_sensor->read()).speed; + motor_speeds.right_drive_speed = HAL_CHECK(m_right.propulsion_speed_sensor->read()).speed; + motor_speeds.right_steer_speed = HAL_CHECK(m_right.steer_speed_sensor->read()).speed; + + return motor_speeds; + } + +private: + + // member variables + + leg& m_left; + leg& m_back; + leg& m_right; +}; +} // namespace Drive diff --git a/drive/platform-implementations/drc_speed_sensor.cpp b/drive/platform-implementations/drc_speed_sensor.cpp index 648306a..785fbed 100644 --- a/drive/platform-implementations/drc_speed_sensor.cpp +++ b/drive/platform-implementations/drc_speed_sensor.cpp @@ -19,4 +19,4 @@ hal::result make_speed_sensor(hal::rmd::drc& p_drc) { return drc_speed_sensor(p_drc); } -} // namespace sjsu::drive \ No newline at end of file +} // namespace sjsu::drive diff --git a/drive/platform-implementations/drc_speed_sensor.hpp b/drive/platform-implementations/drc_speed_sensor.hpp new file mode 100644 index 0000000..349033b --- /dev/null +++ b/drive/platform-implementations/drc_speed_sensor.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include "speed_sensor.hpp" +#include + +namespace sjsu::drive { + +class drc_speed_sensor; + +hal::result make_speed_sensor(hal::rmd::drc& p_drc); + +class drc_speed_sensor : public speed_sensor { +private: + drc_speed_sensor(hal::rmd::drc& p_drc); + hal::result driver_read() override; + friend hal::result make_speed_sensor(hal::rmd::drc& p_drc); + hal::rmd::drc* m_drc = nullptr; +}; +} \ No newline at end of file diff --git a/drive/platform-implementations/esp8266_mission_control.cpp b/drive/platform-implementations/esp8266_mission_control.cpp index 30edf8f..321b8b4 100644 --- a/drive/platform-implementations/esp8266_mission_control.cpp +++ b/drive/platform-implementations/esp8266_mission_control.cpp @@ -244,4 +244,4 @@ hal::status esp8266_mission_control::establish_connection( return hal::success(); } -} // namespace sjsu::drive \ No newline at end of file +} // namespace sjsu::drive diff --git a/drive/platform-implementations/home.hpp b/drive/platform-implementations/home.hpp index b034647..d4f60fc 100644 --- a/drive/platform-implementations/home.hpp +++ b/drive/platform-implementations/home.hpp @@ -68,4 +68,4 @@ inline hal::status home(std::span p_homing_structs, return hal::success(); } -} // namespace sjsu::drive \ No newline at end of file +} // namespace sjsu::drive diff --git a/drive/platform-implementations/mission_control.hpp b/drive/platform-implementations/mission_control.hpp new file mode 100644 index 0000000..0ba3802 --- /dev/null +++ b/drive/platform-implementations/mission_control.hpp @@ -0,0 +1,83 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace sjsu::drive{ + +static constexpr char kResponseBodyFormat[] = + "{\"HB\":%d,\"IO\":%d,\"WO\":%d,\"DM\":\"%c\",\"CMD\":[%d,%d]}\n"; + +static constexpr char kGETRequestFormat[] = + "drive?heartbeat_count=%d&is_operational=%d&wheel_orientation=%d&drive_mode=%" + "c&speed=%d&angle=%d"; + + +class mission_control +{ + public: + struct mc_commands + { + char mode = 'D'; + int speed = 0; + int angle = 0; + int wheel_orientation = 0; + int is_operational = 0; + int heartbeat_count = 0; + hal::status print(hal::serial* terminal) + { + hal::print<128>(*terminal, + "HB: %d\n" + "IS_OP: %d\n" + "Speed: %d\n" + "Angle: %d\n" + "Mode: %c\n" + "Wheel Orientation: %d\n", + heartbeat_count, + is_operational, + speed, + angle, + mode, + wheel_orientation); + return hal::success(); + } + }; + /** + * @brief Get the command object + * + * This command is infallible and will always return a command. + * If mission control is disconnected from the machine, this command will + * return the last known value. The heartbeat_count can be used to indicate + * when the connection has been re-established. + * + * It is the responsibility of this driver to re-establishing the connection. + * This driver is allowed to utilize time during this call in order to + * reestablish the connection. + * + * The timeout is used to break out of the function. This is useful, because + * attempting to reestablish a connection to mission control could take a + * considerable amount of time and thus, could stall out the application. + * + * @param p_timeout - the amount of time to either get the latest command or + * attempt to reestablish a connection to mission control. + * @return drive_commands - the last known driver command received. If no + * commands have been received, then this should return the default + * initialized command. + */ + hal::result get_command(hal::function_ref p_timeout) + { + return impl_get_command(p_timeout); + } + + virtual ~mission_control() = default; + +private: + virtual hal::result impl_get_command( + hal::function_ref p_timeout) = 0; +}; + +} \ No newline at end of file diff --git a/drive/platform-implementations/offset_servo.hpp b/drive/platform-implementations/offset_servo.hpp new file mode 100644 index 0000000..be16235 --- /dev/null +++ b/drive/platform-implementations/offset_servo.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include + +namespace sjsu::drive { + +class offset_servo : public hal::servo { +public: + static hal::result create(hal::servo& p_servo, hal::degrees p_offset) { + return offset_servo(p_servo, p_offset); + } + + hal::degrees get_offset() { + return m_offset; + } + + void set_offset(hal::degrees p_offset) { + m_offset = p_offset; + } + +private: + offset_servo(hal::servo& p_servo, hal::degrees p_offset) : m_servo(&p_servo), m_offset(p_offset) + { + } + + hal::result driver_position(hal::degrees p_position) override { + return HAL_CHECK(m_servo->position(p_position + m_offset)); + } + + hal::degrees m_offset; + hal::servo* m_servo = nullptr; +}; + +} diff --git a/drive/platform-implementations/print_mission_control.hpp b/drive/platform-implementations/print_mission_control.hpp new file mode 100644 index 0000000..14d00ec --- /dev/null +++ b/drive/platform-implementations/print_mission_control.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include "mission_control.hpp" + +#include +#include +#include +#include +#include +#include + +namespace sjsu::drive{ + +class print_mission_control : public mission_control +{ +public: + + static hal::result create(hal::serial& p_console) + { + return print_mission_control(p_console); + } + + hal::result get_command(hal::function_ref p_timeout) + { + return impl_get_command(p_timeout); + } + +private: + + print_mission_control(hal::serial& p_console) : m_console(&p_console) + { + } + + hal::result impl_get_command(hal::function_ref p_timeout) { + hal::print(*m_console, "getting command"); + return mc_commands{}; + } + hal::serial* m_console; +}; + +} diff --git a/drive/platform-implementations/print_motor.hpp b/drive/platform-implementations/print_motor.hpp new file mode 100644 index 0000000..7172c5b --- /dev/null +++ b/drive/platform-implementations/print_motor.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include + +namespace sjsu::drive { + +class print_motor : public hal::motor { +public: + static hal::result create(hal::serial& p_console) { + return print_motor(p_console); + } + +private: + print_motor(hal::serial& p_console) : m_console(&p_console) + { + } + + hal::result driver_power(float p_speed) override { + hal::print<20>(*m_console, "speed: %f\n", p_speed); + return hal::motor::power_t{}; + } + + hal::serial* m_console; +}; + +} diff --git a/drive/platform-implementations/print_servo.hpp b/drive/platform-implementations/print_servo.hpp new file mode 100644 index 0000000..0244112 --- /dev/null +++ b/drive/platform-implementations/print_servo.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include +#include +#include + +namespace sjsu::drive { + +class print_servo : public hal::servo { +public: + static hal::result create(hal::serial& p_console, hal::servo& p_servo) { + return print_servo(p_console, p_servo); + } + +private: + print_servo(hal::serial& p_console, hal::servo& p_servo) : m_console(&p_console) , m_servo(&p_servo) + { + } + + hal::result driver_position(hal::degrees p_position) override { + hal::print<20>(*m_console, "position: %f\n", static_cast(p_position)); + return HAL_CHECK(m_servo->position(p_position)); + } + + hal::serial* m_console; + hal::servo* m_servo; +}; + +} diff --git a/drive/platform-implementations/print_speed_sensor.hpp b/drive/platform-implementations/print_speed_sensor.hpp new file mode 100644 index 0000000..74196c4 --- /dev/null +++ b/drive/platform-implementations/print_speed_sensor.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include "speed_sensor.hpp" +#include +#include +#include + +namespace sjsu::drive { + +class print_speed_sensor : public speed_sensor { +public: + + static hal::result make_speed_sensor(hal::serial& p_console) { + return print_speed_sensor(p_console); + } + +private: + + print_speed_sensor(hal::serial& p_console) + : m_console(&p_console) + { + } + + hal::result driver_read() { + using namespace hal::literals; + + auto speed = 0.0_rpm; + hal::print<100>(*m_console, "speed read: %f\n", speed); + + return speed_sensor::read_t{.speed = 0.0_rpm}; + } + hal::serial* m_console; +}; +} diff --git a/drive/platform-implementations/speed_sensor.hpp b/drive/platform-implementations/speed_sensor.hpp new file mode 100644 index 0000000..c305d46 --- /dev/null +++ b/drive/platform-implementations/speed_sensor.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include + +namespace sjsu::drive { + +class speed_sensor +{ + public: + struct read_t { + hal::rpm speed = 0; + }; + + [[nodiscard]] hal::result read() + { + return driver_read(); + } + + virtual ~speed_sensor() = default; + + private: + virtual hal::result driver_read() = 0; +}; +} \ No newline at end of file diff --git a/science/applications/application.cpp b/science/applications/application.cpp new file mode 100644 index 0000000..45015e2 --- /dev/null +++ b/science/applications/application.cpp @@ -0,0 +1,21 @@ +#include +#include +#include + +#include "application.hpp" +namespace sjsu::science { + +hal::status application(application_framework& p_framework) +{ + using namespace std::literals; + + auto& clock = *p_framework.steady_clock; + auto& console = *p_framework.terminal; + + while (true) { + // Print message + hal::print(console, "Hello, World\n"); + + } +} +} // namespace sjsu::science diff --git a/science/applications/application.hpp b/science/applications/application.hpp new file mode 100644 index 0000000..ebb6102 --- /dev/null +++ b/science/applications/application.hpp @@ -0,0 +1,50 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sjsu::science { +struct application_framework +{ + hal::serial* terminal; + hal::can* can; + hal::input_pin* in_pin0; + hal::input_pin* in_pin1; + hal::input_pin* in_pin2; + hal::pwm* pwm_1_6; + hal::pwm* pwm_1_5; + hal::adc* adc_4; + hal::adc* adc_5; + hal::serial* esp; + hal::i2c* i2c; + hal::steady_clock* steady_clock; + hal::callback reset; +}; + +// Application function must be implemented by one of the compilation units +// (.cpp) files. +hal::status initialize_processor(); +hal::result initialize_platform(); +hal::status application(application_framework& p_framework); + +} // namespace sjsu::science diff --git a/science/demos/CMakeLists.txt b/science/demos/CMakeLists.txt new file mode 100644 index 0000000..8fc537c --- /dev/null +++ b/science/demos/CMakeLists.txt @@ -0,0 +1,34 @@ +# Copyright 2023 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.20) + +project(demos LANGUAGES CXX) + +libhal_build_demos( + DEMOS + co2_demo + pressure_sensor_demo + + SOURCES + ../platform-implementations/pressure_sensor_bme680.cpp + + PACKAGES + libhal-lpc40 + libhal-util + + LINK_LIBRARIES + libhal::lpc40 + libhal::util +) \ No newline at end of file diff --git a/science/demos/applications/pressure_sensor_demo.cpp b/science/demos/applications/pressure_sensor_demo.cpp new file mode 100644 index 0000000..65fa7c8 --- /dev/null +++ b/science/demos/applications/pressure_sensor_demo.cpp @@ -0,0 +1,121 @@ +#include +#include +#include +#include +#include +#include + +#include "../../platform-implementations/pressure_sensor_bme680.hpp" +#include "../hardware_map.hpp" + +using namespace hal::literals; +using namespace std::chrono_literals; + +hal::status probe_bus(hal::i2c& i2c, hal::serial& console) { + hal::print(console, "\n\nProbing i2c2\n"); + for(hal::byte addr = 0x08; addr < 0x78; addr++) { + if (hal::probe(i2c, addr)) { + hal::print<8>(console, "0x%02X ", addr); + }else{ + hal::print(console, " -- "); + } + if(addr % 8 == 7) { + hal::print(console, "\n"); + } + } + hal::print(console, "\n"); + + return hal::success(); +} + + +static int get_bit_value(hal::byte value, hal::byte bit_position) { + return (value >> bit_position) & 0x01; +} + +double calculate_height(double pressure) { + double press_sea = 101325; + double std_tmp = 23 + 273; + double std_tmp_lapse = -0.0065; // K/m + double atmos_layer_h = 0.0; + double R = 8.31432; // Nm/(mol * K) + double g = 9.80665; // m/s^2 + double M = 0.0289644; // kg/mol + + return atmos_layer_h + (std_tmp / std_tmp_lapse) * (pow(pressure / press_sea, -R * std_tmp_lapse / (g * M)) - 1); + +} + + +void print_binary(hal::serial& console, hal::byte val) { + hal::print<11>(console, "0b%c%c%c%c%c%c%c%c", + get_bit_value(val, 7) + '0', // '1' if the bit is set, '0' if its not + get_bit_value(val, 6) + '0', + get_bit_value(val, 5) + '0', + get_bit_value(val, 4) + '0', + get_bit_value(val, 3) + '0', + get_bit_value(val, 2) + '0', + get_bit_value(val, 1) + '0', + get_bit_value(val, 0) + '0'); +} + +namespace sjsu::science { + +hal::status application(application_framework& p_framework) +{ + // configure drivers + auto& i2c = *p_framework.i2c; + auto& clock = *p_framework.steady_clock; + auto& console = *p_framework.terminal; + + HAL_CHECK(probe_bus(i2c, console)); + + auto bme = HAL_CHECK(hal::bme::bme680::create(i2c, 0x77)); + + HAL_CHECK(bme.set_filter_coefficient(hal::bme::bme680::coeff_3)); + HAL_CHECK(bme.set_oversampling(hal::bme::bme680::oversampling_1, hal::bme::bme680::oversampling_2, hal::bme::bme680::oversampling_16)); + + auto readings = HAL_CHECK(bme.get_data()); + double average_pressure = 0.0; + int samples = 100; + for(int i = 0; i < samples; i ++) { + readings = HAL_CHECK(bme.get_data()); + hal::delay(clock, 10ms); + average_pressure += readings.pressure; + } + average_pressure /= samples; + + average_pressure = 0.0; + samples = 100; + for(int i = 0; i < samples; i ++) { + readings = HAL_CHECK(bme.get_data()); + hal::delay(clock, 10ms); + average_pressure += readings.pressure; + } + average_pressure /= samples; + + double init_height = calculate_height(average_pressure); + + print<40>(console, "init height: %fm\n", init_height); + + constexpr int samples_per_read_out = 20; + int i = 0; + while (true) { + + // HAL_CHECK(led.level(true)); + readings = HAL_CHECK(bme.get_data()); + double height = calculate_height(readings.pressure); + + // HAL_CHECK(led.level(false)); + hal::delay(clock, 50ms); + + i++; + if(i % samples_per_read_out == 0) { + hal::print<40>(console, "| °C | %-8s | %-4s | m |\n", "kPa", "%"); + hal::print<40>(console, "| %-3.1f | %-8.2f | %-4.1f | %-4.1f |\n", readings.temperature, average_pressure / 1000, readings.humidity, height - init_height); + } + } + + return hal::success(); +} +} // namespace sjsu::science \ No newline at end of file diff --git a/science/demos/conanfile.py b/science/demos/conanfile.py new file mode 100644 index 0000000..7053c1e --- /dev/null +++ b/science/demos/conanfile.py @@ -0,0 +1,41 @@ +# Copyright 2023 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from conan import ConanFile +from conan.tools.cmake import CMake, cmake_layout + + +class demos(ConanFile): + settings = "compiler", "build_type", "os", "arch" + generators = "CMakeToolchain", "CMakeDeps", "VirtualBuildEnv" + options = {"platform": ["ANY"]} + default_options = {"platform": "unspecified"} + + def build_requirements(self): + self.tool_requires("cmake/3.27.1") + self.tool_requires("libhal-cmake-util/2.2.0") + + def requirements(self): + if str(self.options.platform).startswith("lpc40"): + self.requires("libhal-lpc40/[^2.1.4]") + self.requires("libhal-rmd/3.0.0") + + def layout(self): + platform_directory = "build/" + str(self.options.platform) + cmake_layout(self, build_folder=platform_directory) + + def build(self): + cmake = CMake(self) + cmake.configure() + cmake.build() \ No newline at end of file diff --git a/science/demos/hardware_map.hpp b/science/demos/hardware_map.hpp new file mode 100644 index 0000000..ebb6102 --- /dev/null +++ b/science/demos/hardware_map.hpp @@ -0,0 +1,50 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sjsu::science { +struct application_framework +{ + hal::serial* terminal; + hal::can* can; + hal::input_pin* in_pin0; + hal::input_pin* in_pin1; + hal::input_pin* in_pin2; + hal::pwm* pwm_1_6; + hal::pwm* pwm_1_5; + hal::adc* adc_4; + hal::adc* adc_5; + hal::serial* esp; + hal::i2c* i2c; + hal::steady_clock* steady_clock; + hal::callback reset; +}; + +// Application function must be implemented by one of the compilation units +// (.cpp) files. +hal::status initialize_processor(); +hal::result initialize_platform(); +hal::status application(application_framework& p_framework); + +} // namespace sjsu::science diff --git a/science/demos/main.cpp b/science/demos/main.cpp new file mode 100644 index 0000000..28f5662 --- /dev/null +++ b/science/demos/main.cpp @@ -0,0 +1,42 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "hardware_map.hpp" + +int main() +{ + auto platform_status = sjsu::science::initialize_platform(); + + if (!platform_status) { + hal::halt(); + } + + auto hardware_map = platform_status.value(); + auto is_finished = application(hardware_map); + + if (!is_finished) { + hardware_map.reset(); + } else { + hal::halt(); + } + + return 0; +} + +namespace boost { +void throw_exception(std::exception const&) +{ + hal::halt(); +} +} // namespace boost \ No newline at end of file diff --git a/science/demos/platforms/lpc4078.cpp b/science/demos/platforms/lpc4078.cpp new file mode 100644 index 0000000..f395068 --- /dev/null +++ b/science/demos/platforms/lpc4078.cpp @@ -0,0 +1,104 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +// #include //not sure why we need this? +#include +#include + +#include + +#include "../hardware_map.hpp" +namespace sjsu::science { +hal::status initialize_processor() +{ + hal::cortex_m::initialize_data_section(); + hal::cortex_m::initialize_floating_point_unit(); + + return hal::success(); +} + +hal::result initialize_platform() +{ + using namespace hal::literals; + using namespace std::chrono_literals; + + // Set the MCU to the maximum clock speed + HAL_CHECK(hal::lpc40::clock::maximum(12.0_MHz)); + + auto& clock = hal::lpc40::clock::get(); + auto cpu_frequency = clock.get_frequency(hal::lpc40::peripheral::cpu); + static hal::cortex_m::dwt_counter counter(cpu_frequency); + + // Serial + static std::array recieve_buffer0{}; + static auto uart0 = HAL_CHECK((hal::lpc40::uart::get(0, + recieve_buffer0, + hal::serial::settings{ + .baud_rate = 38400, + }))); + // Don't think we need can for the science applications thus far + static hal::can::settings can_settings{ .baud_rate = 1.0_MHz }; + static auto can = HAL_CHECK((hal::lpc40::can::get(2, can_settings))); + + static auto in_pin0 = + HAL_CHECK(hal::lpc40::input_pin::get(1, 15, hal::input_pin::settings{})); + static auto in_pin1 = + HAL_CHECK(hal::lpc40::input_pin::get(1, 23, hal::input_pin::settings{})); + static auto in_pin2 = + HAL_CHECK(hal::lpc40::input_pin::get(1, 22, hal::input_pin::settings{})); + + static auto pwm_1_6 = HAL_CHECK((hal::lpc40::pwm::get(1, 6))); + static auto pwm_1_5 = HAL_CHECK((hal::lpc40::pwm::get(1, 5))); + + static auto adc_4 = HAL_CHECK(hal::lpc40::adc::get(4)); + static auto adc_5 = HAL_CHECK(hal::lpc40::adc::get(5)); + + std::array receive_buffer{}; + static auto uart1 = HAL_CHECK(hal::lpc40::uart::get(0, + receive_buffer, + { + .baud_rate = 115200.0f, + })); + + static auto i2c = HAL_CHECK(hal::lpc40::i2c::get(2)); + + return application_framework{ + .terminal = &uart0, + .can = &can, + .in_pin0 = &in_pin0, + .in_pin1 = &in_pin1, + .in_pin2 = &in_pin2, + .pwm_1_6 = &pwm_1_6, + .pwm_1_5 = &pwm_1_5, + .adc_4 = &adc_4, + .adc_5 = &adc_5, + .esp = &uart1, + .i2c = &i2c, + .steady_clock = &counter, + .reset = []() { hal::cortex_m::reset(); }, + }; +}; +} // namespace sjsu::science diff --git a/science/platform-implementations/co2_sensor.hpp b/science/platform-implementations/co2_sensor.hpp new file mode 100644 index 0000000..b1a6422 --- /dev/null +++ b/science/platform-implementations/co2_sensor.hpp @@ -0,0 +1 @@ +//Not implemented yet this will be an interface where any pressure sensor object will have to implement these function diff --git a/science/platform-implementations/esp8266_mission_control.cpp b/science/platform-implementations/esp8266_mission_control.cpp new file mode 100644 index 0000000..7eb46f9 --- /dev/null +++ b/science/platform-implementations/esp8266_mission_control.cpp @@ -0,0 +1,317 @@ +#pragma once + +#include "mission_control.hpp" +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace sjsu::science { + +class esp8266_mission_control : public mission_control +{ +public: + static hal::result create( + hal::esp8266::at& p_esp8266, + hal::serial& p_console, + const std::string_view p_ssid, + const std::string_view p_password, + const hal::esp8266::at::socket_config& p_config, + const std::string_view p_ip, + hal::timeout auto& p_timeout, + std::span p_buffer, + std::string_view p_get_request) + { + esp8266_mission_control esp_mission_control = + esp8266_mission_control(p_esp8266, + p_console, + p_ssid, + p_password, + p_config, + p_ip, + p_buffer, + p_get_request); + HAL_CHECK(esp_mission_control.establish_connection(p_timeout)); + + return esp_mission_control; + } + +private: + esp8266_mission_control(hal::esp8266::at& p_esp8266, + hal::serial& p_console, + const std::string_view p_ssid, + const std::string_view p_password, + const hal::esp8266::at::socket_config& p_config, + const std::string_view p_ip, + std::span p_buffer, + std::string_view p_get_request) + : m_esp8266(&p_esp8266) + , m_console(&p_console) + , m_ssid(p_ssid) + , m_password(p_password) + , m_config(p_config) + , m_ip(p_ip) + , m_buffer(p_buffer) + , m_get_request(p_get_request) + , m_fill_payload(hal::stream_fill(m_buffer)) + , m_http_header_parser(new_http_header_parser()) + { + m_buffer_len = 0; + } + + hal::result impl_get_command( + hal::function_ref p_timeout) override + { + using namespace std::literals; + + // auto dumfuck = HAL_CHECK(m_esp8266->is_connected_to_server(p_timeout)); + if (m_write_error) { + hal::print(*m_console, "Reconnecting...\n"); + // Wait 1s before attempting to reconnect + + auto result = establish_connection(p_timeout); + if (!result) { + hal::print(*m_console, "Failure!!!\n"); + return m_commands; + } + + hal::print(*m_console, "CONNECTION RE-ESTABLISHED!!\n"); + m_read_complete = true; + m_buffer_len = 0; + m_content_length = 0; + m_http_header_parser = new_http_header_parser(); + m_write_error = false; + m_header_finished = false; + } + if (m_read_complete) { + + // Send out HTTP GET request + + auto status = + m_esp8266->server_write(hal::as_bytes(m_get_request), p_timeout); + + if (!status) { + hal::print(*m_console, "\nFailed to write to server!\n"); + hal::print(*m_console, m_get_request); + m_write_error = true; + return m_commands; + } + + m_missed_read = 0; + m_read_complete = false; + m_header_finished = false; + } + auto received = HAL_CHECK(m_esp8266->server_read(m_buffer)).data; + auto remainder = received | m_http_header_parser.find_header_start | + m_http_header_parser.find_content_length | + m_http_header_parser.parse_content_length | + m_http_header_parser.find_end_of_header; + + m_missed_read++; + if (m_missed_read > 10) { + hal::print(*m_console, "READ MISS!!!\n"); + m_write_error = true; + m_missed_read = 0; + return m_commands; + } + if (!m_header_finished && + hal::finished(m_http_header_parser.find_end_of_header)) { + m_content_length = m_http_header_parser.parse_content_length.value(); + m_header_finished = true; + std::fill(m_command_buffer.begin(), m_command_buffer.end(), 0); + remainder = remainder.subspan(1); + } + + if (m_header_finished) { + // hal::print<128>(*m_console, " read miss = %u\n", m_missed_read); + + auto tmp = m_content_length - m_buffer_len; + auto byte_to_read = std::min((size_t)tmp, remainder.size()); + + for (int i = 0; i < byte_to_read; i++) { + m_command_buffer[m_buffer_len + i] = remainder[i]; + } + m_buffer_len += byte_to_read; + // hal::print<1024>(*m_console, + // "M header has finished, remainder size: %d, buffer " + // "data: %.*s, buffer len %d, content length %d\n", + // remainder.size(), + // m_buffer_len + 1, + // m_command_buffer.data(), + // m_buffer_len, + // m_content_length); + + if (m_buffer_len >= m_content_length) { + // hal::print(*m_console, "content length has been met \n"); + m_read_complete = true; + m_missed_read = 0; + m_buffer_len = 0; + parse_commands(); + m_http_header_parser = new_http_header_parser(); + } + } + return m_commands; + } + + void parse_commands() { + + auto result = to_string_view(m_command_buffer); + static constexpr int expected_number_of_arguments = 6; + mc_commands commands; + + int actual_arguments = sscanf(result.data(), + kResponseBodyFormat, + &commands.heartbeat_count, + &commands.is_operational, + &commands.wheel_orientation, + &commands.mode, + &commands.speed, + &commands.angle); + if (actual_arguments != expected_number_of_arguments) { + hal::print<200>(*m_console, + "Received %d arguments, expected %d\n", + actual_arguments, + expected_number_of_arguments); + } + // hal::print<200>(*m_console, + // "HB: %d\t, IO %d\t, WO: %d\t, DM: %c\t, Speed: %d\n, Angle: %d\n", + // commands.heartbeat_count, + // commands.is_operational, + // commands.wheel_orientation, + // commands.mode, + // commands.speed, + // commands.angle + // ); + m_commands = commands; + } + + + std::string_view to_string_view(std::span p_span) + { + return std::string_view(reinterpret_cast(p_span.data()), + p_span.size()); + } + + enum class connection_state + { + check_ap_connection, + connecting_to_ap, + set_ip_address, + check_server_connection, + connecting_to_server, + connection_established, + }; + + [[nodiscard]] hal::status establish_connection(hal::timeout auto& p_timeout) + { + connection_state state = connection_state::check_ap_connection; + + while (state != connection_state::connection_established) { + switch (state) { + case connection_state::check_ap_connection: + hal::print(*m_console, "Checking if AP \""); + hal::print(*m_console, m_ssid); + hal::print(*m_console, "\" is connected... "); + if (HAL_CHECK(m_esp8266->is_connected_to_ap(p_timeout))) { + state = connection_state::check_server_connection; + hal::print(*m_console, "Connected!\n"); + } else { + state = connection_state::connecting_to_ap; + hal::print(*m_console, "NOT Connected!\n"); + } + break; + case connection_state::connecting_to_ap: + hal::print(*m_console, "Connecting to AP: \""); + hal::print(*m_console, m_ssid); + hal::print(*m_console, "\" ...\n"); + HAL_CHECK(m_esp8266->connect_to_ap(m_ssid, m_password, p_timeout)); + state = connection_state::set_ip_address; + break; + case connection_state::set_ip_address: + if (!m_ip.empty()) { + hal::print(*m_console, "Setting IP Address to: "); + hal::print(*m_console, m_ip); + hal::print(*m_console, " ...\n"); + HAL_CHECK(m_esp8266->set_ip_address(m_ip, p_timeout)); + } + state = connection_state::check_server_connection; + break; + case connection_state::check_server_connection: + hal::print(*m_console, "Checking if server \""); + hal::print(*m_console, m_config.domain); + hal::print(*m_console, "\" is connected... \n"); + if (HAL_CHECK(m_esp8266->is_connected_to_server(p_timeout))) { + state = connection_state::connection_established; + hal::print(*m_console, "Connected!\n"); + } else { + state = connection_state::connecting_to_server; + hal::print(*m_console, "NOT Connected!\n"); + } + break; + case connection_state::connecting_to_server: + hal::print(*m_console, "Connecting to server: \""); + hal::print(*m_console, m_config.domain); + hal::print(*m_console, "\" ...\n"); + HAL_CHECK(m_esp8266->connect_to_server(m_config, p_timeout)); + hal::print(*m_console, "connected\n"); + state = connection_state::check_server_connection; + break; + case connection_state::connection_established: + // Do nothing, allow next iteration to break while loop + hal::print(*m_console, "Succesfully Connected."); + break; + default: + state = connection_state::connecting_to_ap; + } + } + + return hal::success(); + } + + struct http_header_parser_t + { + hal::stream_find find_header_start; + hal::stream_find find_content_length; + hal::stream_parse parse_content_length; + hal::stream_find find_end_of_header; + }; + + http_header_parser_t new_http_header_parser() + { + using namespace std::literals; + + return http_header_parser_t{ + .find_header_start = hal::stream_find(hal::as_bytes("HTTP/1.1 "sv)), + .find_content_length = + hal::stream_find(hal::as_bytes("Content-Length: "sv)), + .parse_content_length = hal::stream_parse(), + .find_end_of_header = hal::stream_find(hal::as_bytes("\r\n\r\n"sv)), + }; + } + + mc_commands m_commands{}; + hal::esp8266::at* m_esp8266; + hal::serial* m_console; + std::string_view m_ssid; + std::string_view m_password; + const hal::esp8266::at::socket_config& m_config; + std::string_view m_ip; + std::span m_buffer; + std::array m_command_buffer; + std::string_view m_get_request; + http_header_parser_t m_http_header_parser; + size_t m_buffer_len; + bool m_write_error = false; + bool m_header_finished = false; + bool m_read_complete = true; + hal::stream_fill m_fill_payload; + size_t m_content_length; + std::uint32_t m_missed_read = 0; +}; +} // namespace sjsu::arm \ No newline at end of file diff --git a/science/platform-implementations/mission_control.hpp b/science/platform-implementations/mission_control.hpp new file mode 100644 index 0000000..131ded5 --- /dev/null +++ b/science/platform-implementations/mission_control.hpp @@ -0,0 +1,83 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace sjsu::science{ + +static constexpr char kResponseBodyFormat[] = + "{\"HB\":%d,\"IO\":%d,\"WO\":%d,\"DM\":\"%c\",\"CMD\":[%d,%d]}\n"; + +static constexpr char kGETRequestFormat[] = + "drive?heartbeat_count=%d&is_operational=%d&wheel_orientation=%d&drive_mode=%" + "c&speed=%d&angle=%d"; + + +class mission_control +{ + public: + struct mc_commands + { + char mode = 'D'; + int speed = 0; + int angle = 0; + int wheel_orientation = 0; + int is_operational = 0; + int heartbeat_count = 0; + hal::status print(hal::serial* terminal) + { + hal::print<128>(*terminal, + "HB: %d\n" + "IS_OP: %d\n" + "Speed: %d\n" + "Angle: %d\n" + "Mode: %c\n" + "Wheel Orientation: %d\n", + heartbeat_count, + is_operational, + speed, + angle, + mode, + wheel_orientation); + return hal::success(); + } + }; + /** + * @brief Get the command object + * + * This command is infallible and will always return a command. + * If mission control is disconnected from the machine, this command will + * return the last known value. The heartbeat_count can be used to indicate + * when the connection has been re-established. + * + * It is the responsibility of this driver to re-establishing the connection. + * This driver is allowed to utilize time during this call in order to + * reestablish the connection. + * + * The timeout is used to break out of the function. This is useful, because + * attempting to reestablish a connection to mission control could take a + * considerable amount of time and thus, could stall out the application. + * + * @param p_timeout - the amount of time to either get the latest command or + * attempt to reestablish a connection to mission control. + * @return drive_commands - the last known driver command received. If no + * commands have been received, then this should return the default + * initialized command. + */ + hal::result get_command(hal::function_ref p_timeout) + { + return impl_get_command(p_timeout); + } + + virtual ~mission_control() = default; + +private: + virtual hal::result impl_get_command( + hal::function_ref p_timeout) = 0; +}; + +} \ No newline at end of file diff --git a/science/platform-implementations/pressure_sensor.hpp b/science/platform-implementations/pressure_sensor.hpp new file mode 100644 index 0000000..b1a6422 --- /dev/null +++ b/science/platform-implementations/pressure_sensor.hpp @@ -0,0 +1 @@ +//Not implemented yet this will be an interface where any pressure sensor object will have to implement these function diff --git a/science/platform-implementations/pressure_sensor_bme680.cpp b/science/platform-implementations/pressure_sensor_bme680.cpp new file mode 100644 index 0000000..5d33707 --- /dev/null +++ b/science/platform-implementations/pressure_sensor_bme680.cpp @@ -0,0 +1,428 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pressure_sensor_bme680.hpp" +#include + +using namespace hal::literals; +using namespace std::chrono_literals; + + +// static void print_byte_array(hal::serial& console, std::span arr) { +// for(auto& i : arr) { +// hal::print<16>(console, "0x%02x ", i); +// } +// } + +// static void print_hex_range(hal::serial& console, hal::byte start, hal::byte end) { +// for(hal::byte i = start; i < end; i ++) { +// hal::print<16>(console, "0x%02x ", i); +// } +// } + +hal::result hal::bme::bme680::create(hal::i2c& p_i2c, hal::byte p_address) { + bme680 bme(p_i2c, p_address); + + HAL_CHECK(bme.read_addr()); + HAL_CHECK(bme.soft_reset()); + HAL_CHECK(bme.get_calibration_coefficients()); + + return bme; +} + +hal::status hal::bme::bme680::soft_reset() { + HAL_CHECK(write_register(0xE0, 0xB6)); + + return hal::success(); +} + +[[nodiscard]] hal::result hal::bme::bme680::read_addr() { + std::array out; + HAL_CHECK(read_registers(registers::id, out)); + return out[0]; +} + +hal::bme::bme680::bme680(hal::i2c& p_i2c, hal::byte p_address) { + m_i2c = &p_i2c; + m_address = p_address; +} + + +hal::status hal::bme::bme680::set_filter_coefficient(filter_coeff coeff) { + std::array out; + HAL_CHECK(read_registers(registers::config, out)); + + hal::byte original_value = out[0]; + hal::byte new_value = (original_value & (0b11100011)) | (coeff << 2); + + HAL_CHECK(write_register(registers::config, new_value)); + + return hal::success(); +} +hal::status hal::bme::bme680::set_oversampling(oversampling temp_osr,oversampling press_osr, oversampling humid_osr) { + std::array out; + HAL_CHECK(read_registers(registers::ctrl_hum, out)); + + hal::byte ctrl_meas_original_value = out[1]; + hal::byte ctrl_meas_new_value = (ctrl_meas_original_value & (0b00000011)) | (((hal::byte) temp_osr) << 5) | (((hal::byte) press_osr) << 2); + + hal::byte ctrl_hum_original_value = out[0]; + hal::byte ctrl_hum_new_value = (ctrl_hum_original_value & (0b11111000)) | (((hal::byte) humid_osr) << 0); + + HAL_CHECK(write_register(registers::ctrl_hum, ctrl_hum_new_value)); + // hal::delay(*m_steady_clock, 20ms); // Wait 10ms for readings.s + HAL_CHECK(write_register(registers::ctrl_meas, ctrl_meas_new_value)); + + return hal::success(); +} + +hal::status hal::bme::bme680::set_mode(mode p_mode) { + std::array out; + HAL_CHECK(read_registers(registers::ctrl_meas, out)); + + hal::byte original_value = out[0]; + hal::byte new_value = (original_value & (0b11111100)) | (p_mode << 0); + + HAL_CHECK(write_register(registers::ctrl_meas, new_value)); + + return hal::success(); +} + +static int16_t combine(uint8_t upper, uint8_t lower) { + return (((int16_t) upper) << 8) | ((int16_t) lower); +} + +// hal::status hal::bme::bme680::get_calibration_coefficients(hal::serial& console) { +// // CHECK THE DATASHEET +// std::array reg8A_regA0; +// HAL_CHECK(read_registers(0x8A, reg8A_regA0)); +// auto span_reg8A_regA0 = std::span(reg8A_regA0); + +// // span_reg8A_regA0.subspan(0, 11); + +// HAL_CHECK(read_registers(0x8A, span_reg8A_regA0.subspan(0, 8))); + +// std::array regE1_regEE; +// HAL_CHECK(read_registers(0xE1, regE1_regEE)); + +// print_byte_array(console, reg8A_regA0); +// hal::print(console, "\n"); +// print_byte_array(console, regE1_regEE); + +// coefficients.par_t1 = combine(regE1_regEE[0xEA - 0xE1],regE1_regEE[0xE9 - 0xE1]); +// coefficients.par_t2 = combine(reg8A_regA0[0x8B - 0x8A], reg8A_regA0[0x8A - 0x8A]); +// coefficients.par_t3 = combine(0, reg8A_regA0[0x8C - 0x8A]); + + +// coefficients.par_p1 = combine(reg8A_regA0[0x8F - 0x8A], reg8A_regA0[0x8E - 0x8A]); +// coefficients.par_p2 = combine(reg8A_regA0[0x91 - 0x8A], reg8A_regA0[0x90 - 0x8A]); +// coefficients.par_p3 = combine(0, reg8A_regA0[0x90 - 0x8A]); +// coefficients.par_p4 = combine(reg8A_regA0[0x95 - 0x8A], reg8A_regA0[0x94 - 0x8A]); +// coefficients.par_p5 = combine(reg8A_regA0[0x97 - 0x8A], reg8A_regA0[0x96 - 0x8A]); +// coefficients.par_p6 = combine(0, reg8A_regA0[0x99 - 0x8A]); +// coefficients.par_p7 = combine(0, reg8A_regA0[0x98 - 0x8A]); +// coefficients.par_p8 = combine(reg8A_regA0[reg8A_regA0[0x9D - 0x8A]], reg8A_regA0[0x9C - 0x8A]); +// coefficients.par_p9 = combine(reg8A_regA0[0x9F - 0x8A], reg8A_regA0[0x9E - 0x8A]); +// coefficients.par_p10 = combine(0, reg8A_regA0[0xA0 - 0x8A]); + + +// coefficients.par_h1 = (((int16_t) regE1_regEE[0xE3 - 0xE1]) << 4) | (regE1_regEE[0xE2 - 0xE1] & 0x0F); +// coefficients.par_h2 = (((int16_t) regE1_regEE[0xE1 - 0xE1]) << 4) | (regE1_regEE[0xE2 - 0xE1] >> 4); +// coefficients.par_h3 = combine(0, regE1_regEE[0xE4 - 0xE1]); +// coefficients.par_h4 = combine(0, regE1_regEE[0xE5 - 0xE1]); +// coefficients.par_h5 = combine(0, regE1_regEE[0xE6 - 0xE1]); +// coefficients.par_h6 = combine(0, regE1_regEE[0xE7 - 0xE1]); +// coefficients.par_h7 = combine(0, regE1_regEE[0xE8 - 0xE1]); + +// coefficients.par_g1 = combine(0, regE1_regEE[0xED - 0xE1]); +// coefficients.par_g2 = combine(regE1_regEE[0xEC - 0xE1], regE1_regEE[0xEB - 0xE1]); +// coefficients.par_g3 = combine(0, regE1_regEE[0xEE - 0xE1]); + +// return hal::success(); +// } + +hal::status hal::bme::bme680::get_calibration_coefficients() { + // CHECK THE DATASHEET + std::array reg8A_regA0; + HAL_CHECK(read_registers(0x8A, reg8A_regA0)); + + auto span_reg8A_regA0 = std::span(reg8A_regA0); + HAL_CHECK(read_registers(0x8A, span_reg8A_regA0.subspan(0, 8))); + + std::array regE1_regEE; + HAL_CHECK(read_registers(0xE1, regE1_regEE)); + + coefficients.par_t1 = combine(regE1_regEE[0xEA - 0xE1],regE1_regEE[0xE9 - 0xE1]); + coefficients.par_t2 = combine(reg8A_regA0[0x8B - 0x8A], reg8A_regA0[0x8A - 0x8A]); + coefficients.par_t3 = combine(0, reg8A_regA0[0x8C - 0x8A]); + + + coefficients.par_p1 = combine(reg8A_regA0[0x8F - 0x8A], reg8A_regA0[0x8E - 0x8A]); + coefficients.par_p2 = combine(reg8A_regA0[0x91 - 0x8A], reg8A_regA0[0x90 - 0x8A]); + coefficients.par_p3 = combine(0, reg8A_regA0[0x90 - 0x8A]); + coefficients.par_p4 = combine(reg8A_regA0[0x95 - 0x8A], reg8A_regA0[0x94 - 0x8A]); + coefficients.par_p5 = combine(reg8A_regA0[0x97 - 0x8A], reg8A_regA0[0x96 - 0x8A]); + coefficients.par_p6 = combine(0, reg8A_regA0[0x99 - 0x8A]); + coefficients.par_p7 = combine(0, reg8A_regA0[0x98 - 0x8A]); + coefficients.par_p8 = combine(reg8A_regA0[reg8A_regA0[0x9D - 0x8A]], reg8A_regA0[0x9C - 0x8A]); + coefficients.par_p9 = combine(reg8A_regA0[0x9F - 0x8A], reg8A_regA0[0x9E - 0x8A]); + coefficients.par_p10 = combine(0, reg8A_regA0[0xA0 - 0x8A]); + + + coefficients.par_h1 = (((int16_t) regE1_regEE[0xE3 - 0xE1]) << 4) | (regE1_regEE[0xE2 - 0xE1] & 0x0F); + coefficients.par_h2 = (((int16_t) regE1_regEE[0xE1 - 0xE1]) << 4) | (regE1_regEE[0xE2 - 0xE1] >> 4); + coefficients.par_h3 = combine(0, regE1_regEE[0xE4 - 0xE1]); + coefficients.par_h4 = combine(0, regE1_regEE[0xE5 - 0xE1]); + coefficients.par_h5 = combine(0, regE1_regEE[0xE6 - 0xE1]); + coefficients.par_h6 = combine(0, regE1_regEE[0xE7 - 0xE1]); + coefficients.par_h7 = combine(0, regE1_regEE[0xE8 - 0xE1]); + + coefficients.par_g1 = combine(0, regE1_regEE[0xED - 0xE1]); + coefficients.par_g2 = combine(regE1_regEE[0xEC - 0xE1], regE1_regEE[0xEB - 0xE1]); + coefficients.par_g3 = combine(0, regE1_regEE[0xEE - 0xE1]); + + return hal::success(); +} + +// hal::result hal::bme::bme680::get_data(hal::serial& console) { +// // Set the bme680 into forced modes +// HAL_CHECK(set_mode(mode::forced)); +// hal::delay(*m_steady_clock, 20ms); // Wait 10ms for readings. +// print_register(console, registers::ctrl_meas, format::binary); + +// std::array buffer; +// // read_registers(registers::pressure_msb, buffer); +// HAL_CHECK(read_registers(registers::pressure_msb, buffer)); + +// uint32_t humidity_raw = (((uint32_t) buffer[6]) << 8) | ((uint32_t) buffer[7]); +// uint32_t temperature_raw = (((uint32_t) buffer[3]) << 12) | (((uint32_t) buffer[4]) << 4) | ((uint32_t) buffer[5] >> 4); +// uint32_t pressure_raw = (((uint32_t) buffer[0]) << 12) | (((uint32_t) buffer[1]) << 4) | ((uint32_t) buffer[2] >> 4); + +// print_hex_range(console, 0x1F, 0x2B+1); +// hal::print(console, "\n"); +// print_byte_array(console, buffer); +// hal::print(console, "\n"); + +// hal::print<512>(console, "Raw Data:\n\ttemperature: %d\n\tpressure: %d\n\thumidity: %d\n", temperature_raw, pressure_raw, humidity_raw); + +// compensated_temp temp = compensate_temp(temperature_raw); +// hal::print<512>(console, "temp_comp: %f, temp_find: %f\n", temp.t_comp, temp.t_fine); +// double pressure = compensate_pressure(temp.t_fine, pressure_raw); +// double humidity = compensate_humidity(temp.t_comp, humidity_raw); + +// readings_t out; +// out.temperature = temp.t_comp; +// out.pressure = pressure; +// out.humidity = humidity; +// return out; + +// } + + +hal::result hal::bme::bme680::get_data() { + // Set the bme680 into forced mode + HAL_CHECK(set_mode(mode::forced)); + // hal::delay(*m_steady_clock, 1ms); // Wait 1ms for readings. + + + std::array buffer; + HAL_CHECK(read_registers(registers::pressure_msb, buffer)); + + uint32_t humidity_raw = (((uint32_t) buffer[6]) << 8) | ((uint32_t) buffer[7]); + uint32_t temperature_raw = (((uint32_t) buffer[3]) << 12) | (((uint32_t) buffer[4]) << 4) | ((uint32_t) buffer[5] >> 4); + uint32_t pressure_raw = (((uint32_t) buffer[0]) << 12) | (((uint32_t) buffer[1]) << 4) | ((uint32_t) buffer[2] >> 4); + + compensated_temp temp = compensate_temp(temperature_raw); + double pressure = compensate_pressure(temp.t_fine, pressure_raw); + double humidity = compensate_humidity(temp.t_comp, humidity_raw); + + readings_t out; + out.temperature = temp.t_comp; + out.pressure = pressure; + out.humidity = humidity; + return out; +} + +hal::status hal::bme::bme680::write_register(hal::byte register_address, hal::byte value) { + HAL_CHECK(hal::write( + *m_i2c, + m_address, + std::array { register_address, value }, + hal::never_timeout())); + + return hal::success(); +} + +hal::status hal::bme::bme680::read_registers(hal::byte register_address, std::span out) { + HAL_CHECK(hal::write_then_read(*m_i2c, + m_address, + std::array { register_address }, + out, + hal::never_timeout())); + + return hal::success(); +} + +hal::bme::bme680::compensated_temp hal::bme::bme680::compensate_temp(double temp_adc) { + // CHECK THE DATASHEET + double var1 = ((((double)temp_adc) / 16384.0) - (((double)coefficients.par_t1) / 1024.0)) * ((double)coefficients.par_t2); + double var2 = (((((double)temp_adc) / 131072.0) - (((double)coefficients.par_t1) / 8192.0)) * + ((((double)temp_adc) / 131072.0) - (((double)coefficients.par_t1) / 8192.0))) * + (((double)coefficients.par_t3) * 16.0); + compensated_temp out; + out.t_fine = var1 + var2; + out.t_comp = out.t_fine / 5120.0; + return out; +} +double hal::bme::bme680::compensate_pressure(double t_fine, double press_adc) { + // CHECK THE DATASHEET + + double var1 = ((double)t_fine / 2.0) - 64000.0; + double var2 = var1 * var1 * ((double)coefficients.par_p6 / 131072.0); + var2 = var2 + (var1 * (double)coefficients.par_p5 * 2.0); + var2 = (var2 / 4.0) + ((double)coefficients.par_p4 * 65536.0); + var1 = ((((double)coefficients.par_p3 * var1 * var1) / 16384.0) + + ((double)coefficients.par_p2 * var1)) / 524288.0; + var1 = (1.0 + (var1 / 32768.0)) * (double)coefficients.par_p1; + double press_comp = 1048576.0 - (double)press_adc; + press_comp = ((press_comp - (var2 / 4096.0)) * 6250.0) / var1; + var1 = ((double)coefficients.par_p9 * press_comp * press_comp) / 2147483648.0; + var2 = press_comp * ((double)coefficients.par_p8 / 32768.0); + double var3 = (press_comp / 256.0) * (press_comp / 256.0) * + (press_comp / 256.0) * (coefficients.par_p10 / 131072.0); + press_comp = press_comp + (var1 + var2 + var3 + + ((double)coefficients.par_p7 * 128.0)) / 16.0; + return press_comp; +} +double hal::bme::bme680::compensate_humidity(double temp_comp, double hum_adc) { + // CHECK THE DATASHEET + + double var1 = hum_adc - (((double) coefficients.par_h1 * 16.0) + (((double) coefficients.par_h3 / 2.0) * temp_comp)); + double var2 = var1 * (((double) coefficients.par_h2 / 262144.0) * (1.0 + (((double) coefficients.par_h4 / 16384.0) * + temp_comp) + (((double) coefficients.par_h5 / 1048576.0) * temp_comp * temp_comp))); + double var3 = (double) coefficients.par_h6 / 16384.0; + double var4 = (double) coefficients.par_h7 / 2097152.0; + return var2 + ((var3 + (var4 * temp_comp)) * var2 * var2); +} + + +// hal::bme::bme680::compensated_temp hal::bme::bme680::compensate_temp(int32_t temp_adc) { +// // CHECK THE DATASHEET + +// int32_t var1 = ((int32_t)temp_adc >> 3) - ((int32_t)coefficients.par_t1 << 1); +// int32_t var2 = (var1 * (int32_t)coefficients.par_t2) >> 11; +// int32_t var3 = ((((var1 >> 1) * (var1 >> 1)) >> 12) * ((int32_t)coefficients.par_t3 << 4)) >> 14; +// hal::bme::bme680::compensated_temp out; +// out.t_fine = var2 + var3; +// out.t_comp = ((out.t_fine * 5) + 128) >> 8; +// return out; +// } +// int32_t hal::bme::bme680::compensate_pressure(int32_t t_fine, int32_t press_raw) { +// // CHECK THE DATASHEET + +// int32_t var1 = ((int32_t)t_fine >> 1) - 64000; +// int32_t var2 = ((((var1 >> 2) * (var1 >> 2)) >> 11) * (int32_t)coefficients.par_p6) >> 2; +// var2 = var2 + ((var1 * (int32_t)coefficients.par_p5) << 1); +// var2 = (var2 >> 2) + ((int32_t)coefficients.par_p4 << 16); +// var1 = (((((var1 >> 2) * (var1 >> 2)) >> 13) * ((int32_t)coefficients.par_p3 << 5)) >> 3) + (((int32_t) coefficients.par_p2 * var1) >> 1); + +// var1 = var1 >> 18; +// var1 = ((32768 + var1) * (int32_t)coefficients.par_p1) >> 15; +// int32_t press_comp = 1048576 - press_raw; +// press_comp = (uint32_t)((press_comp - (var2 >> 12)) * ((uint32_t)3125)); +// if (press_comp >= (1 << 30)) +// press_comp = ((press_comp / (uint32_t)var1) << 1); +// else +// press_comp = ((press_comp << 1) / (uint32_t)var1); +// var1 = ((int32_t)coefficients.par_p9 * (int32_t)(((press_comp >> 3) * +// (press_comp >> 3)) >> 13)) >> 12; +// var2 = ((int32_t)(press_comp >> 2) * (int32_t)coefficients.par_p8) >> 13; +// int32_t var3 = ((int32_t)(press_comp >> 8) * (int32_t)(press_comp >> 8) * +// (int32_t)(press_comp >> 8) * (int32_t)coefficients.par_p10) >> 17; +// press_comp = (int32_t)(press_comp) + ((var1 + var2 + var3 + ((int32_t)coefficients.par_p7 << 7)) >> 4); +// return press_comp; +// } +// int32_t hal::bme::bme680::compensate_humidity(int32_t temp_comp, int32_t hum_adc) { +// // CHECK THE DATASHEET + +// int32_t temp_scaled = (int32_t)temp_comp; +// int32_t var1 = (int32_t)hum_adc - (int32_t)((int32_t)coefficients.par_h1 << 4) - (((temp_scaled * (int32_t)coefficients.par_h3) / ((int32_t)100)) >> 1); +// int32_t var2 = ((int32_t)coefficients.par_h2 * (((temp_scaled * +// (int32_t)coefficients.par_h4) / ((int32_t)100)) + +// (((temp_scaled * ((temp_scaled * (int32_t)coefficients.par_h5) / +// ((int32_t)100))) >> 6) / ((int32_t)100)) + ((int32_t)(1 << 14)))) >> 10; +// int32_t var3 = var1 * var2; +// int32_t var4 = (((int32_t)coefficients.par_h6 << 7) + +// ((temp_scaled * (int32_t)coefficients.par_h7) / ((int32_t)100))) >> 4; +// int32_t var5 = ((var3 >> 14) * (var3 >> 14)) >> 10; +// int32_t var6 = (var4 * var5) >> 1; +// return (((var3 + var6) >> 10) * ((int32_t) 1000)) >> 12; +// } + + +void hal::bme::bme680::print_calibration_coefficients(hal::serial& console) { + hal::print<512>(console, "Temp Coefficients: %10d %10d %10d\n", coefficients.par_t1, coefficients.par_t2, coefficients.par_t3); + hal::print<512>(console, "Press Coefficients:\n%10d %10d %10d\n%10d %10d %10d\n%10d %10d %10d\n%10d %10d %10d\n%10d\n", coefficients.par_p1, coefficients.par_p2, coefficients.par_p3, coefficients.par_p4, coefficients.par_p5, coefficients.par_p6, coefficients.par_p7, coefficients.par_p8, coefficients.par_p9, coefficients.par_p10); + hal::print<512>(console, "Humidity Coefficients:\n%10d %10d %10d\n%10d %10d %10d\n%10d\n", coefficients.par_h1, coefficients.par_h2, coefficients.par_h3, coefficients.par_h4, coefficients.par_h5, coefficients.par_h6, coefficients.par_h7); + +} + +// static int get_bit_value(hal::byte value, hal::byte bit_position) { +// return (value >> bit_position) & 0x01; +// } + +// void hal::bme::bme680::print_register(hal::serial& console, hal::byte register_address, format format_p) { +// std::array out; +// hal::status result = read_registers(register_address, out); + +// hal::byte reg_val = out[0]; + +// switch(format_p) { +// case binary: +// // 0x12: 0b1010101010\n +// // 0x12: 0b----------\n +// if(result) { +// hal::print<32>(console, "0x%02x: 0b%c%c%c%c%c%c%c%c\n", +// register_address, +// get_bit_value(reg_val, 7) + '0', // '1' if the bit is set, '0' if its not +// get_bit_value(reg_val, 6) + '0', +// get_bit_value(reg_val, 5) + '0', +// get_bit_value(reg_val, 4) + '0', +// get_bit_value(reg_val, 3) + '0', +// get_bit_value(reg_val, 2) + '0', +// get_bit_value(reg_val, 1) + '0', +// get_bit_value(reg_val, 0) + '0' +// ); +// }else { +// hal::print<32>(console, "0x%02x: 0b--------\n", register_address); +// } +// return; +// case decimal: +// if(result) { +// int value = reg_val; +// hal::print<32>(console, "0x%02x: %-3d\n", register_address, value); +// }else { +// hal::print<32>(console, "0x%02x: ---\n", register_address); +// } +// return; +// case hex: +// if(result) { +// int value = reg_val; +// hal::print<32>(console, "0x%02x: 0x%02x\n", register_address, value); +// }else { +// hal::print<32>(console, "0x%02x: 0x--\n", register_address); +// } +// return; +// } +// } diff --git a/science/platform-implementations/pressure_sensor_bme680.hpp b/science/platform-implementations/pressure_sensor_bme680.hpp new file mode 100644 index 0000000..9c8bf4e --- /dev/null +++ b/science/platform-implementations/pressure_sensor_bme680.hpp @@ -0,0 +1,220 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +// enum format { +// binary, decimal, hex +// }; + +namespace hal::bme { +class bme680 +{ +public: + + /// @brief Possible modes + enum mode : hal::byte { + sleep = 0b00, + forced = 0b01, + }; + + /// @brief Possible oversampling values + enum oversampling : hal::byte { + oversampling_1 = 0b001, + oversampling_2 = 0b010, + oversampling_4 = 0b011, + oversampling_8 = 0b100, + oversampling_16 = 0b0101, + }; + + /// @brief Possible coefficients for the on board iir filter + enum filter_coeff : hal::byte { + coeff_0 = 0b000, + coeff_1 = 0b001, + coeff_3 = 0b010, + coeff_7 = 0b011, + coeff_15 = 0b100, + coeff_31 = 0b101, + coeff_63 = 0b110, + coeff_127 = 0b111, + }; + + /// @brief A list of named registers on the bme680 + enum registers : hal::byte { + status = 0x73, + reset = 0xE0, + id = 0xD0, + config = 0x75, + ctrl_meas = 0x74, + ctrl_hum = 0x72, + ctrl_gas_1 = 0x71, + ctrl_gas_0 = 0x70, + // gas_wait_x = 0x70, + // ctrl_gas_1 = 0x72, + gas_r_lsb = 0x2B, + gas_r_msb = 0x2A, + humidity_lsb = 0x26, + humidity_msb = 0x25, + temp_xlsb = 0x24, + temp_lsb = 0x23, + temp_msb = 0x22, + pressure_xlsb = 0x21, + pressure_lsb = 0x20, + pressure_msb = 0x1F, + eas_status_0 = 0x1D, + + }; + + /// The device address when 0x76 is connected to GND. + static constexpr hal::byte address_ground = 0x76; + + /// @brief Create a new bme680 on an i2c bus at a given address. + /// @param p_i2c i2c bus of the device + /// @param p_address address of the bme680, defaults to 0x76 + /// @return + static result create(hal::i2c& p_i2c, hal::byte p_address = address_ground); + + + /// @brief Reads the "Chip Ident" register + /// @return Should return the address of the chip + [[nodiscard]] hal::result read_addr(); + + + /// @brief Reset the sensor + /// @return + [[nodiscard]] hal::status soft_reset(); + + /// @brief Set the coefficient value of the onboard iir filter. + /// @param coeff + /// @return v + [[nodiscard]] hal::status set_filter_coefficient(filter_coeff coeff); + /// @brief Set the oversampling values for each sensor. + /// @param temp_osr Temperature oversampling + /// @param press_osr Pressure oversampling + /// @param humid_osr Humidity oversampling + /// @return + [[nodiscard]] hal::status set_oversampling(oversampling temp_osr,oversampling press_osr, oversampling humid_osr); + + /// @brief Set the current mode of the sensor + /// @param p_mode + /// @return + [[nodiscard]] hal::status set_mode(mode p_mode); + + /// @brief Gets the calibration coefficients on the chip + /// @return + [[nodiscard]] hal::status get_calibration_coefficients(); + + /// @brief Readings from the sensor + struct readings_t { + double temperature = 0; + double pressure = 0; + double humidity = 0; + }; + + /// @brief Sets the mode to forced and reads a single measurement. + /// @return The readings from the sensor + [[nodiscard]] hal::result get_data(); + + /// @brief Print calibration coefficients. For debugging + /// @param console Console to print to + void print_calibration_coefficients(hal::serial& console); + + + // /// @brief Print a register. For debugging + // /// @param console Console to print to. + // /// @param register_address Register to read + // /// @param format Format to print register as. + // void print_register(hal::serial& console, hal::byte register_address, format format = format::hex); + +private: + /// @brief Construct a bme680 + /// @param p_i2c bus + /// @param p_address address + explicit bme680(hal::i2c& p_i2c, hal::byte p_address); + + /// @brief Write a single byte to a register + /// @param register_address Address of register + /// @param value Value to write + /// @return + [[nodiscard]] hal::status write_register(hal::byte register_address, hal::byte value); + /// @brief Burst read bytes starting from a register. Reads towards higher memory. + /// @param register_address Address of register to begin reading from. + /// @param out Buffer to store read bytes. + /// @return + [[nodiscard]] hal::status read_registers(hal::byte register_address, std::span out); + + /// @brief Structure with compensated and fine temp. Used internally. Check the datasheet. + struct compensated_temp{ + double t_comp; + double t_fine; + }; + + /// @brief Structure to hold coefficient values read during get_calibration_coefficients(). Check the datasheet. + struct { + uint16_t par_t1; + int16_t par_t2; + int8_t par_t3; + + uint16_t par_p1; + int16_t par_p2; + int8_t par_p3; + int16_t par_p4; + int16_t par_p5; + int8_t par_p6; + int8_t par_p7; + int16_t par_p8; + int16_t par_p9; + uint8_t par_p10; + + int16_t par_h1; + int16_t par_h2; + int16_t par_h3; + int16_t par_h4; + int16_t par_h5; + int16_t par_h6; + + int16_t par_h7; + + int16_t par_g1; + int16_t par_g2; + int16_t par_g3; + + } coefficients; + + /// @brief Correct temperature according to datasheet and calibration coeffcients. Check the datasheet. + /// @param adc_temp Raw temperature reading from sensor + /// @return compensated and fine temperature. + compensated_temp compensate_temp(double adc_temp); + /// @brief Correct pressure according to datasheet, fine temperature and calibration coefficients. Check the datasheet. + /// @param t_fine Fine temperature calculated during compensate_temp() + /// @param adc_press Raw pressure reading from sensor. + /// @return Compensated pressure + double compensate_pressure(double t_fine, double adc_press); + /// @brief Correct humidity according to datasheet, compensated temperature, and calibration coefficienst. Check the datasheet. + /// @param temp_comp Compensated temperature calculated during compensate_temp() + /// @param adc_humid Raw humidity reading from sensor. + /// @return Compensated humidity. + double compensate_humidity(double temp_comp, double adc_humid); + + /// @brief I2C bus the device is on + hal::i2c* m_i2c; + + /// @brief I2C address of the bme680 + hal::byte m_address; +}; + +} // namespace hal::mpu \ No newline at end of file From 467456107967998a15847986b71595858f13195b Mon Sep 17 00:00:00 2001 From: Coreyboy1820 <92711317+Coreyboy1820@users.noreply.github.com> Date: Fri, 12 Jan 2024 16:24:34 -0800 Subject: [PATCH 5/6] :sparkles: Added CI for windows and Ubuntu building (#106) --------- Co-authored-by: Viha123 Co-authored-by: FluffyFTW Co-authored-by: vihashah Co-authored-by: Andrew Lin --- .github/workflows/build.yml | 69 +++++++++++++++++++++++++++---------- 1 file changed, 51 insertions(+), 18 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 8cc6724..beff870 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -13,42 +13,75 @@ on: - cron: '0 12 * * 0' jobs: - deploy: + build_ubuntu: strategy: fail-fast: false - matrix: - include: - - os: ubuntu-22.04 - profile_name: x86_64/linux - - os: macos-12 - profile_name: x86_64/mac - - os: windows-2022 - profile_name: x86_64/windows - runs-on: ${{ matrix.os }} + runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v2 with: submodules: true fetch-depth: 0 + - name: Install GCC, clang tidy, and build essentials + run: sudo apt update && sudo apt upgrade; sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test; sudo apt install -y build-essential g++-11 clang-tidy + - name: 📥 Install CMake & Conan - run: python3 -m pip install "conan>=2.0.6" cmake + run: python3 -m pip install "conan>=2.0.13" cmake - name: 📡 Add `libhal-trunk` conan remote run: conan remote add libhal-trunk https://libhal.jfrog.io/artifactory/api/conan/trunk-conan - - name: Setting up conan profile - run: conan profile detect --force - run: conan config install -sf profiles/${{matrix.profile_name}}/ -tf profiles https://github.com/libhal/conan-config.git - run: conan config install -sf conan/profiles/ -tf profiles https://github.com/libhal/libhal-armcortex.git - run: conan config install -sf conan/profiles/ -tf profiles https://github.com/libhal/libhal-lpc40.git + run: conan config install -sf profiles/baremetal https://github.com/libhal/conan-config.git; conan profile detect --force; conan config install -sf profiles/x86_64/linux/ -tf profiles https://github.com/libhal/conan-config.git; + + - name: Get conan profiles + run: conan config install -sf conan/profiles/ -tf profiles https://github.com/libhal/libhal-armcortex.git; conan config install -sf conan/profiles/ -tf profiles https://github.com/libhal/libhal-lpc40.git - name: Build drive working-directory: drive - run: conan build . -pr lpc4078 -s build_type=MinSizeRel -b missing + run: conan build . -pr lpc4078 -s build_type=MinSizeRel - name: Build arm working-directory: arm - run: conan build . -pr lpc4078 -s build_type=MinSizeRel -b missing + run: conan build . -pr lpc4078 -s build_type=MinSizeRel + + build_windows: + strategy: + fail-fast: false + runs-on: windows-2022 + steps: + - uses: actions/checkout@v2 + with: + submodules: true + fetch-depth: 0 + + - name: Installing choco + run: Set-ExecutionPolicy Bypass -Scope Process -Force; [System.Net.ServicePointManager]::SecurityProtocol = [System.Net.ServicePointManager]::SecurityProtocol -bor 3072; iex ((New-Object System.Net.WebClient).DownloadString('https://community.chocolatey.org/install.ps1')) + - name: Installing python + run: choco install python + + - name: Installing gcc + run: choco install mingw + + - name: installing make + run: choco install make + + - name: Installing conan and cmake + run: python3 -m pip install -U "conan>=2.0.13" cmake + + - name: setting up conan + run: conan remote add libhal-trunk https://libhal.jfrog.io/artifactory/api/conan/trunk-conan; conan config install -sf profiles/baremetal https://github.com/libhal/conan-config.git; conan profile detect --force; conan config install -sf profiles/x86_64/windows/ -tf profiles https://github.com/libhal/conan-config.git + + - name: Importing lpc profiles + run: conan config install -sf conan/profiles/ -tf profiles https://github.com/libhal/libhal-armcortex.git; conan config install -sf conan/profiles/ -tf profiles https://github.com/libhal/libhal-lpc40.git + + - name: Build drive + working-directory: drive + run: conan build . -pr lpc4078 -s build_type=MinSizeRel + + - name: Build arm + working-directory: arm + run: conan build . -pr lpc4078 -s build_type=MinSizeRel + \ No newline at end of file From c6e208501aeca3d7a5aa5f3506ff2d99f3d491e2 Mon Sep 17 00:00:00 2001 From: Nina Wang <115374853+ninaw04@users.noreply.github.com> Date: Fri, 12 Jan 2024 17:32:17 -0800 Subject: [PATCH 6/6] =?UTF-8?q?=F0=9F=91=B7=20added=20m1=20mac,=20mimic,?= =?UTF-8?q?=20and=20science=20builds=20(#107)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/build.yml | 70 ++++++++++++++++++++++++++++++++++++- 1 file changed, 69 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index beff870..5dfc2d9 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -45,6 +45,14 @@ jobs: - name: Build arm working-directory: arm run: conan build . -pr lpc4078 -s build_type=MinSizeRel + + - name: Build arm-mimic + working-directory: arm_mimic + run: conan build . -pr lpc4078 -s build_type=MinSizeRel + + - name: Build science + working-directory: science + run: conan build . -pr lpc4078 -s build_type=MinSizeRel build_windows: strategy: @@ -84,4 +92,64 @@ jobs: - name: Build arm working-directory: arm run: conan build . -pr lpc4078 -s build_type=MinSizeRel - \ No newline at end of file + + - name: Build arm-mimic + working-directory: arm_mimic + run: conan build . -pr lpc4078 -s build_type=MinSizeRel + + - name: Build science + working-directory: science + run: conan build . -pr lpc4078 -s build_type=MinSizeRel + + build_mac_intel: + strategy: + fail-fast: false + runs-on: macos-latest + steps: + - uses: actions/checkout@v2 + with: + submodules: true + fetch-depth: 0 + + - name: Install Homebrew + run: /bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)" + + - name: Troubleshooting + run: rm '/usr/local/bin/2to3' + + - name: Install latest version of Python 3.x and llvm + run: brew install python@3.11; brew install llvm + + - name: Make clang-tidy available on the command line + run: sudo ln -s $(brew --prefix llvm)/bin/clang-tidy /usr/local/bin/ + + - name: 📥 Install CMake & Conan + run: python3 -m pip install "conan>=2.0.13" cmake + + - name: 📡 Add `libhal-trunk` conan remote + run: conan remote add libhal-trunk https://libhal.jfrog.io/artifactory/api/conan/trunk-conan + + - name: Setting up conan profile + run: conan config install -sf profiles/baremetal https://github.com/libhal/conan-config.git; conan profile detect --force; conan config install -sf profiles/x86_64/linux/ -tf profiles https://github.com/libhal/conan-config.git; + + - name: Get conan profiles + run: conan config install -sf conan/profiles/ -tf profiles https://github.com/libhal/libhal-armcortex.git; conan config install -sf conan/profiles/ -tf profiles https://github.com/libhal/libhal-lpc40.git + + - name: Intel default configuration + run: conan config install -sf profiles/x86_64/mac/ -tf profiles https://github.com/libhal/conan-config.git + + - name: Build drive + working-directory: drive + run: conan build . -pr lpc4078 -s build_type=MinSizeRel + + - name: Build arm + working-directory: arm + run: conan build . -pr lpc4078 -s build_type=MinSizeRel + + - name: Build arm-mimic + working-directory: arm_mimic + run: conan build . -pr lpc4078 -s build_type=MinSizeRel + + - name: Build science + working-directory: science + run: conan build . -pr lpc4078 -s build_type=MinSizeRel \ No newline at end of file