From 4f970c86f113fcd7a23bd3c2572e5c5171489e79 Mon Sep 17 00:00:00 2001 From: Gabe Koleszar <72774655+gabekole@users.noreply.github.com> Date: Sat, 5 Oct 2024 10:37:46 -0400 Subject: [PATCH 01/10] Add calibration mode --- hardware/RF.cpp | 8 ++++++++ include/Controller.hpp | 2 +- include/RF.hpp | 10 +++++++--- src/Mode.cpp | 45 ++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 61 insertions(+), 4 deletions(-) diff --git a/hardware/RF.cpp b/hardware/RF.cpp index b14ddd9..9646402 100644 --- a/hardware/RF.cpp +++ b/hardware/RF.cpp @@ -174,6 +174,14 @@ RF::Command RF::GetCommand() // Will check for commands and return the received return RF::Command::Ignite; else if(input_line == "Release\n") return RF::Command::Release; + else if(input_line == "IncrementXTVC\n") + return RF::Command::IncrementXTVC; + else if(input_line == "IncrementYTVC\n") + return RF::Command::IncrementYTVC; + else if(input_line == "DecrementXTVC\n") + return RF::Command::DecrementXTVC; + else if(input_line == "DecrementYTVC\n") + return RF::Command::DecrementYTVC; else return RF::Command::None; } diff --git a/include/Controller.hpp b/include/Controller.hpp index 6d42ba6..66eb1f2 100644 --- a/include/Controller.hpp +++ b/include/Controller.hpp @@ -16,7 +16,6 @@ class Controller{ //int kNumberControllerGains = 10; private: - TVC tvc; std::vector> euler_queue; Eigen::Matrix x_control; Eigen::Matrix controller_gains; @@ -27,6 +26,7 @@ class Controller{ int current_iteration_index = 0; public: + TVC tvc; Eigen::Vector2d input; double loopTime = 0.005; Controller(TVC& tvc); diff --git a/include/RF.hpp b/include/RF.hpp index d8da433..e266584 100644 --- a/include/RF.hpp +++ b/include/RF.hpp @@ -22,10 +22,14 @@ class RF { None, ABORT, Startup, - TestTVC, - GoIdle, + TestTVC, + GoIdle, Ignite, - Release + Release, + IncrementXTVC, + IncrementYTVC, + DecrementXTVC, + DecrementYTVC, }; diff --git a/src/Mode.cpp b/src/Mode.cpp index ac27952..80dd146 100644 --- a/src/Mode.cpp +++ b/src/Mode.cpp @@ -6,6 +6,9 @@ #include "Telemetry.hpp" #include "RF.hpp" #include +#include +#include +#include //CONSTANTS TO BE FIGURED OUT LATER int abort_threshold = 1; @@ -29,8 +32,50 @@ Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controll int milliseconds_since_start = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time).count(); double seconds = milliseconds_since_start / 1000.0; + static float XTVC = 0.0; + static float YTVC = 0.0; RF::Command command = RF::GetInstance().GetCommand(); + + + if(command == RF::Command::IncrementXTVC){ + XTVC += 0.1; + ostringstream os; + os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; + string s = os.str(); + Telemetry::GetInstance().Log(s); + controller.tvc.SetXTVC(XTVC); + return Mode::Calibration; + } + if(command == RF::Command::IncrementYTVC){ + YTVC += 0.1; + ostringstream os; + os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; + string s = os.str(); + Telemetry::GetInstance().Log(s); + controller.tvc.SetYTVC(XTVC); + return Mode::Calibration; + } + + if(command == RF::Command::DecrementXTVC){ + XTVC -= 0.1; + ostringstream os; + os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; + string s = os.str(); + Telemetry::GetInstance().Log(s); + controller.tvc.SetXTVC(XTVC); + return Mode::Calibration; + } + if(command == RF::Command::DecrementYTVC){ + YTVC -= 0.1; + ostringstream os; + os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; + string s = os.str(); + Telemetry::GetInstance().Log(s); + controller.tvc.SetYTVC(XTVC); + return Mode::Calibration; + } + if(command == RF::Command::TestTVC){ Telemetry::GetInstance().Log("Switching mode from calibration to test tvc"); controller.ImportControlParameters("../k_matrix.csv"); From fc18ffaacbeefd29a22678dec0dbb004290ce8de Mon Sep 17 00:00:00 2001 From: Gabe Koleszar <72774655+gabekole@users.noreply.github.com> Date: Sat, 5 Oct 2024 10:56:26 -0400 Subject: [PATCH 02/10] Fix center angle tvc set --- hardware/TVC.cpp | 2 +- include/RF.hpp | 2 +- src/Mode.cpp | 2 -- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/hardware/TVC.cpp b/hardware/TVC.cpp index ada286e..ec2f0d4 100644 --- a/hardware/TVC.cpp +++ b/hardware/TVC.cpp @@ -12,7 +12,7 @@ void TVC::SetXServo(double angle_rad) double degrees = angle_rad * MissionConstants::kRad2Deg; double servoAngle = -.000095801*powf(degrees, 4) - .0027781*powf(degrees, 3) + .0012874*powf(degrees, 2) - 3.1271*degrees -16.129; - servoAngle += 90 + MissionConstants::kTvcYCenterAngle; + servoAngle += 90 + MissionConstants::kTvcXCenterAngle; servoAngle = (servoAngle < 0) ? 0 : servoAngle; servoAngle = (servoAngle > 180) ? 180 : servoAngle; diff --git a/include/RF.hpp b/include/RF.hpp index e266584..4fa1589 100644 --- a/include/RF.hpp +++ b/include/RF.hpp @@ -40,7 +40,7 @@ class RF { uint16_t mode; float euler[3]; float velocity[3]; - float input[2]; + float input[2]; float dt; uint32_t footer; diff --git a/src/Mode.cpp b/src/Mode.cpp index 80dd146..642f2e8 100644 --- a/src/Mode.cpp +++ b/src/Mode.cpp @@ -36,8 +36,6 @@ Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controll static float YTVC = 0.0; RF::Command command = RF::GetInstance().GetCommand(); - - if(command == RF::Command::IncrementXTVC){ XTVC += 0.1; ostringstream os; From ba40a8d6765bb5289a6a82d1496aca0df131df08 Mon Sep 17 00:00:00 2001 From: Gabe Koleszar <72774655+gabekole@users.noreply.github.com> Date: Sat, 5 Oct 2024 11:07:24 -0400 Subject: [PATCH 03/10] pins --- hardware/TVC.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware/TVC.cpp b/hardware/TVC.cpp index ec2f0d4..65a6e43 100644 --- a/hardware/TVC.cpp +++ b/hardware/TVC.cpp @@ -17,7 +17,7 @@ void TVC::SetXServo(double angle_rad) servoAngle = (servoAngle > 180) ? 180 : servoAngle; double dPulseWidth = 1000 + (servoAngle * 1000 / 180.0); - gpioServo(23, round(dPulseWidth)); + gpioServo(16, round(dPulseWidth)); } void TVC::SetYServo(double angle_rad) @@ -32,5 +32,5 @@ void TVC::SetYServo(double angle_rad) servoAngle = (servoAngle > 180) ? 180 : servoAngle; double dPulseWidth = 1000 + (servoAngle * 1000 / 180.0); - gpioServo(24, round(dPulseWidth)); + gpioServo(18, round(dPulseWidth)); } From 1c0330aaf742fe3566d5a99ee271808e98962c4f Mon Sep 17 00:00:00 2001 From: Gabe Koleszar <72774655+gabekole@users.noreply.github.com> Date: Sat, 5 Oct 2024 11:27:46 -0400 Subject: [PATCH 04/10] Update pin --- src/Main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Main.cpp b/src/Main.cpp index d08dbd7..dce5666 100644 --- a/src/Main.cpp +++ b/src/Main.cpp @@ -30,7 +30,7 @@ int main() gpioSetMode(6, PI_OUTPUT); gpioSetMode(18, PI_OUTPUT); - gpioSetMode(13, PI_OUTPUT); + gpioSetMode(16, PI_OUTPUT); gpioWrite(6, 1); From 8b911f4947f64fa36b47bae51d66121c0ee2ce76 Mon Sep 17 00:00:00 2001 From: jackbogatz <114309871+jackbogatz@users.noreply.github.com> Date: Sat, 5 Oct 2024 11:32:19 -0400 Subject: [PATCH 05/10] Changed variable and function names (#34) --- hardware_test/TVC.cpp | 4 ++-- include/Controller.hpp | 5 +---- include/Navigation.hpp | 2 +- include/TVC.hpp | 1 - src/Controller.cpp | 9 +++------ src/Navigation.cpp | 14 ++++++-------- 6 files changed, 13 insertions(+), 22 deletions(-) diff --git a/hardware_test/TVC.cpp b/hardware_test/TVC.cpp index f602578..1a5721b 100644 --- a/hardware_test/TVC.cpp +++ b/hardware_test/TVC.cpp @@ -4,7 +4,7 @@ #include -void TVC::SetXServo(double dAngle) +void TVC::SetTVCX(double dAngle) { dAngle += 90 + MissionConstants::kTvcYCenterAngle; dAngle = (dAngle < 0) ? 0 : dAngle; @@ -16,7 +16,7 @@ void TVC::SetXServo(double dAngle) -void TVC::SetYServo(double dAngle) +void TVC::SetTVCY(double dAngle) { dAngle += 90 + MissionConstants::kTvcYCenterAngle; dAngle = (dAngle < 0) ? 0 : dAngle; diff --git a/include/Controller.hpp b/include/Controller.hpp index 66eb1f2..34af532 100644 --- a/include/Controller.hpp +++ b/include/Controller.hpp @@ -12,9 +12,6 @@ class Controller{ -//Constant to be figured out later -//int kNumberControllerGains = 10; - private: std::vector> euler_queue; Eigen::Matrix x_control; @@ -37,7 +34,7 @@ class Controller{ void stabilizeAtOffset(Navigation& navigation, double current_time, double offset); void UpdateSafe(); - void CalculateK(double startTime); + void GetNextController_Gain_Time_Index(double startTime); void CalculateInput(); Eigen::Vector2d TvcMath(Eigen::Vector2d input); void Start(double current_time); diff --git a/include/Navigation.hpp b/include/Navigation.hpp index 6216d62..5ee5956 100644 --- a/include/Navigation.hpp +++ b/include/Navigation.hpp @@ -25,7 +25,7 @@ class Navigation void reset(); Eigen::Matrix GetNavigation(); // Defintion of state matrix: TODO (determine dimensions and document form) void UpdateNavigation(); // Defintion updates: TODO (determine dimensions and document form) - std::tuple ComputeAngularRollingAverage(); + std::tuple ComputeAngularRollingAverage(std::vector d_theta_now); Eigen::Matrix3d CreateRotationalMatrix(double phi, double theta, double psi); double GetHeight(); std::tuple GetBodyAcceleration(); diff --git a/include/TVC.hpp b/include/TVC.hpp index 31c1793..5a04637 100644 --- a/include/TVC.hpp +++ b/include/TVC.hpp @@ -21,7 +21,6 @@ class TVC public: TVC() = default; - void SetTVCX(double angle_rad); void SetTVCY(double angle_rad); }; \ No newline at end of file diff --git a/src/Controller.cpp b/src/Controller.cpp index d583473..50e9b17 100644 --- a/src/Controller.cpp +++ b/src/Controller.cpp @@ -85,7 +85,7 @@ void Controller::stabilizeAtOffset(Navigation& navigation, double current_time, } // Extract roll and pitch from stateEstimate, and put it into euler_queue - std::vector currentAngle {stateEstimate(6), stateEstimate(7)}; + std::vector currentAngle = {stateEstimate(6), stateEstimate(7)}; euler_queue.push_back(currentAngle); // Determine if euler_queue apprahced its limit, and if so, delelete its first entry @@ -107,7 +107,7 @@ void Controller::stabilizeAtOffset(Navigation& navigation, double current_time, if(current_iteration_index < MissionConstants::kNumberControllerGains - 1){ - CalculateK(current_time); + GetNextController_Gain_Time_Index(current_time); } // Calculate what angle we need to tell the tvc to move if (current_time > next_tvc_time){ @@ -123,7 +123,7 @@ void Controller::UpdateSafe(){ // TODO. Center TVC, turn off reaction wheel, etc. } -void Controller::CalculateK(double current_time){ +void Controller::GetNextController_Gain_Time_Index(double current_time){ // Determine if a certain amount of time has passed, and if so, then increase the current_iteration_index and get the next K value double switch_time = (controller_gain_times[current_iteration_index+1] - controller_gain_times[current_iteration_index]) / 2.0; @@ -150,9 +150,6 @@ void Controller::CalculateInput(){ tvc.SetTVCY(input(1)); } - - - void Controller::Center(){ // Center the tvc input(0) = 0; diff --git a/src/Navigation.cpp b/src/Navigation.cpp index 33ef1c4..4f97e68 100644 --- a/src/Navigation.cpp +++ b/src/Navigation.cpp @@ -76,23 +76,22 @@ void Navigation::UpdateNavigation(){ d_theta_now[1] = std::get<1>(angularRate)*cos(phi) - std::get<2>(angularRate)*sin(phi); d_theta_now[2] = std::get<1>(angularRate)*sin(phi)* (1/(cos(theta))) + std::get<2>(angularRate)*cos(phi)* (1/(cos(theta))); //std::cout<< d_theta_now[0] << "\n"; - // Append the vector, d_theta_now, to d_theta_queue_reckon - d_theta_queue_reckon.push_back(d_theta_now); - // Call ComputeAngularRollingAverage to sum up all of the data so far for p,q,r which represent the angular velocity in x, y, and z direction - std::tuple rollingAngularAverage = ComputeAngularRollingAverage(); + std::tuple rollingAngularAverage = ComputeAngularRollingAverage(d_theta_now); // std::cout<< std::get<0>(rollingAngularAverage) << ", " << std::get<1>(rollingAngularAverage) << ", " << std::get<2>(rollingAngularAverage)<<"\n"; + //Assign the sum to their respective states, that being p,q, and r stateMat(9) = std::get<0>(rollingAngularAverage); stateMat(10) = std::get<1>(rollingAngularAverage); stateMat(11) = std::get<2>(rollingAngularAverage); - - } -std::tuple Navigation::ComputeAngularRollingAverage(){ +std::tuple Navigation::ComputeAngularRollingAverage(std::vector d_theta_now){ // Computes a rolling average of the angular velocities // + + // Append the vector, d_theta_now, to d_theta_queue_reckon + d_theta_queue_reckon.push_back(d_theta_now); // Calculate the maximum amount of entries that d_theta_queue_reckon can have int max_theta_dot_smooth_entries = MissionConstants::kNavThetaDotSmooth/loopTime; @@ -113,7 +112,6 @@ std::tuple Navigation::ComputeAngularRollingAverage(){ r += d_theta_queue_reckon[i][2] / d_theta_queue_reckon.size(); } - // Return a tuple of the rolling average of p,q, and r return std::make_tuple(p,q,r); } From a0545e8a8949d42f9a04f5a382e30939034b4750 Mon Sep 17 00:00:00 2001 From: Gabe Koleszar <72774655+gabekole@users.noreply.github.com> Date: Sat, 5 Oct 2024 11:48:08 -0400 Subject: [PATCH 06/10] Fix prints --- src/Mode.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/Mode.cpp b/src/Mode.cpp index 642f2e8..b0c4a2c 100644 --- a/src/Mode.cpp +++ b/src/Mode.cpp @@ -38,18 +38,18 @@ Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controll RF::Command command = RF::GetInstance().GetCommand(); if(command == RF::Command::IncrementXTVC){ XTVC += 0.1; - ostringstream os; + std::ostringstream os; os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; - string s = os.str(); + std::string s = os.str(); Telemetry::GetInstance().Log(s); controller.tvc.SetXTVC(XTVC); return Mode::Calibration; } if(command == RF::Command::IncrementYTVC){ YTVC += 0.1; - ostringstream os; + std::ostringstream os; os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; - string s = os.str(); + std::string s = os.str(); Telemetry::GetInstance().Log(s); controller.tvc.SetYTVC(XTVC); return Mode::Calibration; @@ -57,18 +57,18 @@ Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controll if(command == RF::Command::DecrementXTVC){ XTVC -= 0.1; - ostringstream os; + std::ostringstream os; os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; - string s = os.str(); + std::string s = os.str(); Telemetry::GetInstance().Log(s); controller.tvc.SetXTVC(XTVC); return Mode::Calibration; } if(command == RF::Command::DecrementYTVC){ YTVC -= 0.1; - ostringstream os; + std::ostringstream os; os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; - string s = os.str(); + std::string s = os.str(); Telemetry::GetInstance().Log(s); controller.tvc.SetYTVC(XTVC); return Mode::Calibration; From 55fc15a771335fd064098bcddfcad386060df873 Mon Sep 17 00:00:00 2001 From: Gabe Koleszar <72774655+gabekole@users.noreply.github.com> Date: Sat, 5 Oct 2024 11:49:34 -0400 Subject: [PATCH 07/10] Typo --- src/Mode.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Mode.cpp b/src/Mode.cpp index b0c4a2c..0f09ba5 100644 --- a/src/Mode.cpp +++ b/src/Mode.cpp @@ -42,7 +42,7 @@ Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controll os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; std::string s = os.str(); Telemetry::GetInstance().Log(s); - controller.tvc.SetXTVC(XTVC); + controller.tvc.SetTVCX(XTVC); return Mode::Calibration; } if(command == RF::Command::IncrementYTVC){ @@ -51,7 +51,7 @@ Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controll os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; std::string s = os.str(); Telemetry::GetInstance().Log(s); - controller.tvc.SetYTVC(XTVC); + controller.tvc.SetTVCY(XTVC); return Mode::Calibration; } @@ -61,7 +61,7 @@ Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controll os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; std::string s = os.str(); Telemetry::GetInstance().Log(s); - controller.tvc.SetXTVC(XTVC); + controller.tvc.SetTVCX(XTVC); return Mode::Calibration; } if(command == RF::Command::DecrementYTVC){ @@ -70,7 +70,7 @@ Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controll os << "TVC Position, X: " << std::to_string(XTVC) << " Y: " << std::to_string(YTVC) << std::endl; std::string s = os.str(); Telemetry::GetInstance().Log(s); - controller.tvc.SetYTVC(XTVC); + controller.tvc.SetTVCY(XTVC); return Mode::Calibration; } From af91ebb88fecca58f21d1137491bc33e50a44599 Mon Sep 17 00:00:00 2001 From: Gabe Koleszar <72774655+gabekole@users.noreply.github.com> Date: Sat, 5 Oct 2024 11:55:35 -0400 Subject: [PATCH 08/10] Fix typo --- hardware/TVC.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware/TVC.cpp b/hardware/TVC.cpp index 65a6e43..ab93456 100644 --- a/hardware/TVC.cpp +++ b/hardware/TVC.cpp @@ -7,7 +7,7 @@ #include -void TVC::SetXServo(double angle_rad) +void TVC::SetTVCX(double angle_rad) { double degrees = angle_rad * MissionConstants::kRad2Deg; double servoAngle = -.000095801*powf(degrees, 4) - .0027781*powf(degrees, 3) + .0012874*powf(degrees, 2) - 3.1271*degrees -16.129; @@ -20,7 +20,7 @@ void TVC::SetXServo(double angle_rad) gpioServo(16, round(dPulseWidth)); } -void TVC::SetYServo(double angle_rad) +void TVC::SetTVCY(double angle_rad) { double degrees = angle_rad * MissionConstants::kRad2Deg; From 4e99b170a6769afc035197c7346f392a96b4895f Mon Sep 17 00:00:00 2001 From: Gabe Koleszar <72774655+gabekole@users.noreply.github.com> Date: Tue, 8 Oct 2024 03:26:02 +0000 Subject: [PATCH 09/10] Update timing code --- Makefile | 424 ------------------------------------- hardware_simulation/RF.cpp | 26 ++- hardware_test/RF.cpp | 26 ++- include/Mode.hpp | 12 +- src/Mode.cpp | 79 ++++--- 5 files changed, 82 insertions(+), 485 deletions(-) delete mode 100644 Makefile diff --git a/Makefile b/Makefile deleted file mode 100644 index 4eb3156..0000000 --- a/Makefile +++ /dev/null @@ -1,424 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 3.25 - -# Default target executed when no arguments are given to make. -default_target: all -.PHONY : default_target - -# Allow only one "make -f Makefile2" at a time, but pass parallelism. -.NOTPARALLEL: - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - -# Disable VCS-based implicit rules. -% : %,v - -# Disable VCS-based implicit rules. -% : RCS/% - -# Disable VCS-based implicit rules. -% : RCS/%,v - -# Disable VCS-based implicit rules. -% : SCCS/s.% - -# Disable VCS-based implicit rules. -% : s.% - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Command-line flag to silence nested $(MAKE). -$(VERBOSE)MAKESILENT = -s - -#Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E rm -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/gabekole/ferda - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/gabekole/ferda - -#============================================================================= -# Targets provided globally by CMake. - -# Special rule for the target edit_cache -edit_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..." - /usr/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available. -.PHONY : edit_cache - -# Special rule for the target edit_cache -edit_cache/fast: edit_cache -.PHONY : edit_cache/fast - -# Special rule for the target rebuild_cache -rebuild_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." - /usr/bin/cmake --regenerate-during-build -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) -.PHONY : rebuild_cache - -# Special rule for the target rebuild_cache -rebuild_cache/fast: rebuild_cache -.PHONY : rebuild_cache/fast - -# The main all target -all: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/gabekole/ferda/CMakeFiles /home/gabekole/ferda//CMakeFiles/progress.marks - $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 all - $(CMAKE_COMMAND) -E cmake_progress_start /home/gabekole/ferda/CMakeFiles 0 -.PHONY : all - -# The main clean target -clean: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 clean -.PHONY : clean - -# The main clean target -clean/fast: clean -.PHONY : clean/fast - -# Prepare targets for installation. -preinstall: all - $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall - -# Prepare targets for installation. -preinstall/fast: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall/fast - -# clear depends -depend: - $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 -.PHONY : depend - -#============================================================================= -# Target rules for targets named Ferda - -# Build rule for target. -Ferda: cmake_check_build_system - $(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 Ferda -.PHONY : Ferda - -# fast build rule for target. -Ferda/fast: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/build -.PHONY : Ferda/fast - -hardware/Barometer.o: hardware/Barometer.cpp.o -.PHONY : hardware/Barometer.o - -# target to build an object file -hardware/Barometer.cpp.o: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/Barometer.cpp.o -.PHONY : hardware/Barometer.cpp.o - -hardware/Barometer.i: hardware/Barometer.cpp.i -.PHONY : hardware/Barometer.i - -# target to preprocess a source file -hardware/Barometer.cpp.i: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/Barometer.cpp.i -.PHONY : hardware/Barometer.cpp.i - -hardware/Barometer.s: hardware/Barometer.cpp.s -.PHONY : hardware/Barometer.s - -# target to generate assembly for a file -hardware/Barometer.cpp.s: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/Barometer.cpp.s -.PHONY : hardware/Barometer.cpp.s - -hardware/IMU.o: hardware/IMU.cpp.o -.PHONY : hardware/IMU.o - -# target to build an object file -hardware/IMU.cpp.o: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/IMU.cpp.o -.PHONY : hardware/IMU.cpp.o - -hardware/IMU.i: hardware/IMU.cpp.i -.PHONY : hardware/IMU.i - -# target to preprocess a source file -hardware/IMU.cpp.i: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/IMU.cpp.i -.PHONY : hardware/IMU.cpp.i - -hardware/IMU.s: hardware/IMU.cpp.s -.PHONY : hardware/IMU.s - -# target to generate assembly for a file -hardware/IMU.cpp.s: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/IMU.cpp.s -.PHONY : hardware/IMU.cpp.s - -hardware/Igniter.o: hardware/Igniter.cpp.o -.PHONY : hardware/Igniter.o - -# target to build an object file -hardware/Igniter.cpp.o: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/Igniter.cpp.o -.PHONY : hardware/Igniter.cpp.o - -hardware/Igniter.i: hardware/Igniter.cpp.i -.PHONY : hardware/Igniter.i - -# target to preprocess a source file -hardware/Igniter.cpp.i: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/Igniter.cpp.i -.PHONY : hardware/Igniter.cpp.i - -hardware/Igniter.s: hardware/Igniter.cpp.s -.PHONY : hardware/Igniter.s - -# target to generate assembly for a file -hardware/Igniter.cpp.s: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/Igniter.cpp.s -.PHONY : hardware/Igniter.cpp.s - -hardware/RF.o: hardware/RF.cpp.o -.PHONY : hardware/RF.o - -# target to build an object file -hardware/RF.cpp.o: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/RF.cpp.o -.PHONY : hardware/RF.cpp.o - -hardware/RF.i: hardware/RF.cpp.i -.PHONY : hardware/RF.i - -# target to preprocess a source file -hardware/RF.cpp.i: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/RF.cpp.i -.PHONY : hardware/RF.cpp.i - -hardware/RF.s: hardware/RF.cpp.s -.PHONY : hardware/RF.s - -# target to generate assembly for a file -hardware/RF.cpp.s: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/RF.cpp.s -.PHONY : hardware/RF.cpp.s - -hardware/TVC.o: hardware/TVC.cpp.o -.PHONY : hardware/TVC.o - -# target to build an object file -hardware/TVC.cpp.o: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/TVC.cpp.o -.PHONY : hardware/TVC.cpp.o - -hardware/TVC.i: hardware/TVC.cpp.i -.PHONY : hardware/TVC.i - -# target to preprocess a source file -hardware/TVC.cpp.i: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/TVC.cpp.i -.PHONY : hardware/TVC.cpp.i - -hardware/TVC.s: hardware/TVC.cpp.s -.PHONY : hardware/TVC.s - -# target to generate assembly for a file -hardware/TVC.cpp.s: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/hardware/TVC.cpp.s -.PHONY : hardware/TVC.cpp.s - -src/Controller.o: src/Controller.cpp.o -.PHONY : src/Controller.o - -# target to build an object file -src/Controller.cpp.o: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Controller.cpp.o -.PHONY : src/Controller.cpp.o - -src/Controller.i: src/Controller.cpp.i -.PHONY : src/Controller.i - -# target to preprocess a source file -src/Controller.cpp.i: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Controller.cpp.i -.PHONY : src/Controller.cpp.i - -src/Controller.s: src/Controller.cpp.s -.PHONY : src/Controller.s - -# target to generate assembly for a file -src/Controller.cpp.s: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Controller.cpp.s -.PHONY : src/Controller.cpp.s - -src/Main.o: src/Main.cpp.o -.PHONY : src/Main.o - -# target to build an object file -src/Main.cpp.o: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Main.cpp.o -.PHONY : src/Main.cpp.o - -src/Main.i: src/Main.cpp.i -.PHONY : src/Main.i - -# target to preprocess a source file -src/Main.cpp.i: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Main.cpp.i -.PHONY : src/Main.cpp.i - -src/Main.s: src/Main.cpp.s -.PHONY : src/Main.s - -# target to generate assembly for a file -src/Main.cpp.s: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Main.cpp.s -.PHONY : src/Main.cpp.s - -src/Mode.o: src/Mode.cpp.o -.PHONY : src/Mode.o - -# target to build an object file -src/Mode.cpp.o: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Mode.cpp.o -.PHONY : src/Mode.cpp.o - -src/Mode.i: src/Mode.cpp.i -.PHONY : src/Mode.i - -# target to preprocess a source file -src/Mode.cpp.i: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Mode.cpp.i -.PHONY : src/Mode.cpp.i - -src/Mode.s: src/Mode.cpp.s -.PHONY : src/Mode.s - -# target to generate assembly for a file -src/Mode.cpp.s: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Mode.cpp.s -.PHONY : src/Mode.cpp.s - -src/Navigation.o: src/Navigation.cpp.o -.PHONY : src/Navigation.o - -# target to build an object file -src/Navigation.cpp.o: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Navigation.cpp.o -.PHONY : src/Navigation.cpp.o - -src/Navigation.i: src/Navigation.cpp.i -.PHONY : src/Navigation.i - -# target to preprocess a source file -src/Navigation.cpp.i: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Navigation.cpp.i -.PHONY : src/Navigation.cpp.i - -src/Navigation.s: src/Navigation.cpp.s -.PHONY : src/Navigation.s - -# target to generate assembly for a file -src/Navigation.cpp.s: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Navigation.cpp.s -.PHONY : src/Navigation.cpp.s - -src/Telemetry.o: src/Telemetry.cpp.o -.PHONY : src/Telemetry.o - -# target to build an object file -src/Telemetry.cpp.o: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Telemetry.cpp.o -.PHONY : src/Telemetry.cpp.o - -src/Telemetry.i: src/Telemetry.cpp.i -.PHONY : src/Telemetry.i - -# target to preprocess a source file -src/Telemetry.cpp.i: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Telemetry.cpp.i -.PHONY : src/Telemetry.cpp.i - -src/Telemetry.s: src/Telemetry.cpp.s -.PHONY : src/Telemetry.s - -# target to generate assembly for a file -src/Telemetry.cpp.s: - $(MAKE) $(MAKESILENT) -f CMakeFiles/Ferda.dir/build.make CMakeFiles/Ferda.dir/src/Telemetry.cpp.s -.PHONY : src/Telemetry.cpp.s - -# Help Target -help: - @echo "The following are some of the valid targets for this Makefile:" - @echo "... all (the default if no target is provided)" - @echo "... clean" - @echo "... depend" - @echo "... edit_cache" - @echo "... rebuild_cache" - @echo "... Ferda" - @echo "... hardware/Barometer.o" - @echo "... hardware/Barometer.i" - @echo "... hardware/Barometer.s" - @echo "... hardware/IMU.o" - @echo "... hardware/IMU.i" - @echo "... hardware/IMU.s" - @echo "... hardware/Igniter.o" - @echo "... hardware/Igniter.i" - @echo "... hardware/Igniter.s" - @echo "... hardware/RF.o" - @echo "... hardware/RF.i" - @echo "... hardware/RF.s" - @echo "... hardware/TVC.o" - @echo "... hardware/TVC.i" - @echo "... hardware/TVC.s" - @echo "... src/Controller.o" - @echo "... src/Controller.i" - @echo "... src/Controller.s" - @echo "... src/Main.o" - @echo "... src/Main.i" - @echo "... src/Main.s" - @echo "... src/Mode.o" - @echo "... src/Mode.i" - @echo "... src/Mode.s" - @echo "... src/Navigation.o" - @echo "... src/Navigation.i" - @echo "... src/Navigation.s" - @echo "... src/Telemetry.o" - @echo "... src/Telemetry.i" - @echo "... src/Telemetry.s" -.PHONY : help - - - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system - diff --git a/hardware_simulation/RF.cpp b/hardware_simulation/RF.cpp index c2e58cb..a42517a 100644 --- a/hardware_simulation/RF.cpp +++ b/hardware_simulation/RF.cpp @@ -62,18 +62,30 @@ RF::Command RF::GetCommand() // Will check for commands and return the received std::cout << "GOT: " << input_line << "\n" << std::flush; // if statement for which command to return + RF::Command ParsedCommand = RF::Command::None; + if(input_line == "ABORT") - return RF::Command::ABORT; + ParsedCommand = RF::Command::ABORT; else if(input_line == "Startup") - return RF::Command::Startup; + ParsedCommand = RF::Command::Startup; else if(input_line == "TestTVC") - return RF::Command::TestTVC; + ParsedCommand = RF::Command::TestTVC; else if(input_line == "GoIdle") - return RF::Command::GoIdle; + ParsedCommand = RF::Command::GoIdle; else if(input_line == "Ignite") - return RF::Command::Ignite; + ParsedCommand = RF::Command::Ignite; + else if(input_line == "IncrementYTVC") + ParsedCommand = RF::Command::IncrementYTVC; + else if(input_line == "IncrementXTVC") + ParsedCommand = RF::Command::IncrementXTVC; + else if(input_line == "DecrementXTVC") + ParsedCommand = RF::Command::DecrementXTVC; + else if(input_line == "DecrementYTVC") + ParsedCommand = RF::Command::DecrementYTVC; else if(input_line == "Release") - return RF::Command::Release; + ParsedCommand = RF::Command::Release; else - return RF::Command::None; + ParsedCommand = RF::Command::None; + + return ParsedCommand; } \ No newline at end of file diff --git a/hardware_test/RF.cpp b/hardware_test/RF.cpp index bef2555..a42517a 100644 --- a/hardware_test/RF.cpp +++ b/hardware_test/RF.cpp @@ -62,14 +62,30 @@ RF::Command RF::GetCommand() // Will check for commands and return the received std::cout << "GOT: " << input_line << "\n" << std::flush; // if statement for which command to return + RF::Command ParsedCommand = RF::Command::None; + if(input_line == "ABORT") - return RF::Command::ABORT; + ParsedCommand = RF::Command::ABORT; else if(input_line == "Startup") - return RF::Command::Startup; + ParsedCommand = RF::Command::Startup; + else if(input_line == "TestTVC") + ParsedCommand = RF::Command::TestTVC; + else if(input_line == "GoIdle") + ParsedCommand = RF::Command::GoIdle; else if(input_line == "Ignite") - return RF::Command::Ignite; + ParsedCommand = RF::Command::Ignite; + else if(input_line == "IncrementYTVC") + ParsedCommand = RF::Command::IncrementYTVC; + else if(input_line == "IncrementXTVC") + ParsedCommand = RF::Command::IncrementXTVC; + else if(input_line == "DecrementXTVC") + ParsedCommand = RF::Command::DecrementXTVC; + else if(input_line == "DecrementYTVC") + ParsedCommand = RF::Command::DecrementYTVC; else if(input_line == "Release") - return RF::Command::Release; + ParsedCommand = RF::Command::Release; else - return RF::Command::None; + ParsedCommand = RF::Command::None; + + return ParsedCommand; } \ No newline at end of file diff --git a/include/Mode.hpp b/include/Mode.hpp index a5bbb77..fe258a5 100644 --- a/include/Mode.hpp +++ b/include/Mode.hpp @@ -27,13 +27,11 @@ class Mode private: Mode::Phase eCurrentMode; - Mode::Phase UpdateCalibration(Navigation& navigation, Controller& controller); - - Mode::Phase UpdateTestTVC(Navigation& navigation, Controller& controller); - - Mode::Phase UpdateIdle(Navigation& navigation, Controller& controller); + Mode::Phase UpdateCalibration(Navigation& navigation, Controller& controller, double currentTime); + Mode::Phase UpdateTestTVC(Navigation& navigation, Controller& controller, double currentTime); + Mode::Phase UpdateIdle(Navigation& navigation, Controller& controller, double currentTime); Mode::Phase UpdateLaunch(Navigation& navigation, Controller& controller, Igniter& igniter, double current_time); - Mode::Phase UpdateFreefall(Navigation& navigation, Igniter& igniter, double currTime); + Mode::Phase UpdateFreefall(Navigation& navigation, Igniter& igniter, double currentTime); Mode::Phase UpdateLand(Navigation& navigation, Controller& controller, double current_time); - Mode::Phase UpdateSafeMode(Navigation& navigation, Controller& controller); + Mode::Phase UpdateSafeMode(Navigation& navigation, Controller& controller, double currentTime); }; diff --git a/src/Mode.cpp b/src/Mode.cpp index 0f09ba5..c2b1aa3 100644 --- a/src/Mode.cpp +++ b/src/Mode.cpp @@ -27,11 +27,7 @@ double time_till_second_ignite = 0; Mode::Mode(Phase eInitialMode) : eCurrentMode(eInitialMode) {} -Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controller) { - static auto start_time = std::chrono::high_resolution_clock::now(); - int milliseconds_since_start = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time).count(); - double seconds = milliseconds_since_start / 1000.0; - +Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controller, double currentTime) { static float XTVC = 0.0; static float YTVC = 0.0; @@ -94,21 +90,20 @@ Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controll return Mode::Calibration; } -Mode::Phase Mode::UpdateTestTVC(Navigation& navigation, Controller& controller) { - - static auto start_time = std::chrono::high_resolution_clock::now(); - int milliseconds_since_start = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time).count(); +Mode::Phase Mode::UpdateTestTVC(Navigation& navigation, Controller& controller, double currentTime) { - float seconds = milliseconds_since_start / 1000.0; - controller.UpdateTestTVC(seconds); + static double startTime = currentTime; + double seconds_since_start = currentTime - startTime; + + controller.UpdateTestTVC(seconds_since_start); RF::Command command = RF::GetInstance().GetCommand(); if(command == RF::Command::ABORT){ Telemetry::GetInstance().Log("ABORT, EXITING"); exit(0); } - else if(seconds >= 10){ + else if(seconds_since_start >= 10){ Telemetry::GetInstance().Log("Switching mode from test to idle"); controller.Center(); return Mode::Idle; @@ -119,7 +114,7 @@ Mode::Phase Mode::UpdateTestTVC(Navigation& navigation, Controller& controller) -Mode::Phase Mode::UpdateIdle(Navigation& navigation, Controller& controller) { +Mode::Phase Mode::UpdateIdle(Navigation& navigation, Controller& controller, double currentTime) { navigation.UpdateNavigation(); @@ -141,26 +136,25 @@ Mode::Phase Mode::UpdateIdle(Navigation& navigation, Controller& controller) { return Mode::Idle; } -Mode::Phase Mode::UpdateLaunch(Navigation& navigation, Controller& controller, Igniter& igniter, double current_time) { - - static auto start_time = std::chrono::high_resolution_clock::now(); - int milliseconds_since_start = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time).count(); - +Mode::Phase Mode::UpdateLaunch(Navigation& navigation, Controller& controller, Igniter& igniter, double currentTime) { // Launch rocket and start Controller on first iteration - static int iteration = 1; - if(iteration > 0){ + static double startTime = currentTime; + double seconds_since_start = currentTime - startTime; + + static int startup = 1; + if(startup == 1){ Telemetry::GetInstance().Log("Igniting MOTOR"); igniter.Ignite(Igniter::IgnitionSpecifier::LAUNCH); - controller.Start(current_time); - iteration--; + controller.Start(startTime); + startup = 0; } - if(milliseconds_since_start > 50){ + if(seconds_since_start > 0.050){ igniter.DisableIgnite(Igniter::IgnitionSpecifier::LAUNCH); } navigation.UpdateNavigation(); - controller.UpdateLaunch(navigation, current_time); + controller.UpdateLaunch(navigation, seconds_since_start); // Eigen::Matrix testState = navigation.GetNavigation(); @@ -178,7 +172,7 @@ Mode::Phase Mode::UpdateLaunch(Navigation& navigation, Controller& controller, I // } } -Mode::Phase Mode::UpdateFreefall(Navigation& navigation, Igniter& igniter, double currTime) { +Mode::Phase Mode::UpdateFreefall(Navigation& navigation, Igniter& igniter, double currentTime) { // Continue to update navigation navigation.UpdateNavigation(); @@ -189,7 +183,8 @@ Mode::Phase Mode::UpdateFreefall(Navigation& navigation, Igniter& igniter, doubl Eigen::Matrix currentState = navigation.GetNavigation(); // If the current time is greater than the calibration time + motor thrust duration + and offset, then figure out the best time to ignite - if(currTime > MissionConstants::kFSWCalibrationTime + motor_thrust_duration + offset){ + // TODO THIS LOGIC IS BAD, CURRENT TIME IS VARIABLE DEPENDING ON LAUNCH PROCEDURE + if(currentTime > MissionConstants::kFSWCalibrationTime + motor_thrust_duration + offset){ double a = -9.81/2; double b = currentState(5) + (-9.81*(motor_thrust_duration+motor_thrust_percentage)); @@ -220,11 +215,11 @@ Mode::Phase Mode::UpdateFreefall(Navigation& navigation, Igniter& igniter, doubl } -Mode::Phase Mode::UpdateLand(Navigation& navigation, Controller& controller, double currTime){ +Mode::Phase Mode::UpdateLand(Navigation& navigation, Controller& controller, double currentTime){ // Continue to update navigation and controller navigation.UpdateNavigation(); - controller.UpdateLand(navigation, currTime); + controller.UpdateLand(navigation, currentTime); // If the get height method returns a value between 0 and 1, then we have landed and can go to Safe. if (0.0 < navigation.GetHeight() && navigation.GetHeight() < 1.0) @@ -240,7 +235,7 @@ Mode::Phase Mode::UpdateLand(Navigation& navigation, Controller& controller, dou return Mode::Land; } -Mode::Phase Mode::UpdateSafeMode(Navigation& navigation, Controller& controller){ +Mode::Phase Mode::UpdateSafeMode(Navigation& navigation, Controller& controller, double currentTime){ //continue collection data navigation.UpdateNavigation(); @@ -249,21 +244,21 @@ Mode::Phase Mode::UpdateSafeMode(Navigation& navigation, Controller& controller) bool Mode::Update(Navigation& navigation, Controller& controller, Igniter& igniter) { - static double currTime = 0; - // Make sure the time between last_time and time_now is 0.005 and then keep on adding that change_time to currTime + // Track total elapsed time and delta time + static double currentTime = 0; static auto last_time = std::chrono::high_resolution_clock::now(); auto time_now = std::chrono::high_resolution_clock::now(); - double change_time = std::chrono::duration_cast(time_now - last_time).count() / 1000.0; + unsigned int nanoseconds_since_start = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - last_time).count(); + double change_time = nanoseconds_since_start / 1000000000.0; last_time = time_now; - - currTime += change_time; + + currentTime += change_time; // Update navigations and controller's loopTime to be changeTime which should be 0.005 each time navigation.loopTime = change_time; controller.loopTime = change_time; - // TODO [MOVE TO FUNCTION CALLS FOR CONTROLLER AND NAV] /* Handle behavior based on current phase. Update phase*/ @@ -271,30 +266,30 @@ bool Mode::Update(Navigation& navigation, Controller& controller, Igniter& ignit { case Calibration: Telemetry::GetInstance().RunTelemetry(navigation, controller, 0.05, 0.08); - this->eCurrentMode = UpdateCalibration(navigation, controller); + this->eCurrentMode = UpdateCalibration(navigation, controller, currentTime); break; case TestTVC: Telemetry::GetInstance().RunTelemetry(navigation, controller, 0.05, 0.08); - this->eCurrentMode = UpdateTestTVC(navigation, controller); + this->eCurrentMode = UpdateTestTVC(navigation, controller, currentTime); break; case Idle: Telemetry::GetInstance().RunTelemetry(navigation, controller, 0.05, 0.08); - this->eCurrentMode = UpdateIdle(navigation, controller); + this->eCurrentMode = UpdateIdle(navigation, controller, currentTime); break; case Launch: Telemetry::GetInstance().RunTelemetry(navigation, controller, 0.01, 0.08); - this->eCurrentMode = UpdateLaunch(navigation, controller, igniter, currTime); + this->eCurrentMode = UpdateLaunch(navigation, controller, igniter, currentTime); break; case Freefall: Telemetry::GetInstance().RunTelemetry(navigation, controller, 0.01, 0.08); - this->eCurrentMode = UpdateFreefall(navigation, igniter, currTime); + this->eCurrentMode = UpdateFreefall(navigation, igniter, currentTime); break; case Land: Telemetry::GetInstance().RunTelemetry(navigation, controller, 0.01, 0.08); - this->eCurrentMode = UpdateLand(navigation, controller, currTime); + this->eCurrentMode = UpdateLand(navigation, controller, currentTime); break; case Safe: - this->eCurrentMode = UpdateSafeMode(navigation, controller); + this->eCurrentMode = UpdateSafeMode(navigation, controller, currentTime); break; case Terminate: return false; From dd6f93fd96a7c7e1686a85f56912d3ebb3e54716 Mon Sep 17 00:00:00 2001 From: Gabe Koleszar <72774655+gabekole@users.noreply.github.com> Date: Tue, 8 Oct 2024 17:33:28 -0400 Subject: [PATCH 10/10] Centralize command parsing --- hardware/RF.cpp | 25 +------------------------ hardware_simulation/RF.cpp | 28 +--------------------------- hardware_test/RF.cpp | 28 +--------------------------- include/RF.hpp | 37 ++++++++++++++++++++++++++++++++++++- 4 files changed, 39 insertions(+), 79 deletions(-) diff --git a/hardware/RF.cpp b/hardware/RF.cpp index 9646402..696d519 100644 --- a/hardware/RF.cpp +++ b/hardware/RF.cpp @@ -149,7 +149,6 @@ RF::Command RF::GetCommand() // Will check for commands and return the received std::cout << "GOT: " << buffer; std::string input_line(buffer); - tcflush(SerialFd, TCIFLUSH); @@ -161,27 +160,5 @@ RF::Command RF::GetCommand() // Will check for commands and return the received } std::cout << std::endl; - // if statement for which command to return - if(input_line == "ABORT\n") - return RF::Command::ABORT; - else if(input_line == "Startup\n") - return RF::Command::Startup; - else if(input_line == "TestTVC\n") - return RF::Command::TestTVC; - else if(input_line == "GoIdle\n") - return RF::Command::GoIdle; - else if(input_line == "Ignite\n") - return RF::Command::Ignite; - else if(input_line == "Release\n") - return RF::Command::Release; - else if(input_line == "IncrementXTVC\n") - return RF::Command::IncrementXTVC; - else if(input_line == "IncrementYTVC\n") - return RF::Command::IncrementYTVC; - else if(input_line == "DecrementXTVC\n") - return RF::Command::DecrementXTVC; - else if(input_line == "DecrementYTVC\n") - return RF::Command::DecrementYTVC; - else - return RF::Command::None; + return ParseCommand(input_line); } diff --git a/hardware_simulation/RF.cpp b/hardware_simulation/RF.cpp index a42517a..6aae288 100644 --- a/hardware_simulation/RF.cpp +++ b/hardware_simulation/RF.cpp @@ -61,31 +61,5 @@ RF::Command RF::GetCommand() // Will check for commands and return the received std::getline(std::cin, input_line); std::cout << "GOT: " << input_line << "\n" << std::flush; - // if statement for which command to return - RF::Command ParsedCommand = RF::Command::None; - - if(input_line == "ABORT") - ParsedCommand = RF::Command::ABORT; - else if(input_line == "Startup") - ParsedCommand = RF::Command::Startup; - else if(input_line == "TestTVC") - ParsedCommand = RF::Command::TestTVC; - else if(input_line == "GoIdle") - ParsedCommand = RF::Command::GoIdle; - else if(input_line == "Ignite") - ParsedCommand = RF::Command::Ignite; - else if(input_line == "IncrementYTVC") - ParsedCommand = RF::Command::IncrementYTVC; - else if(input_line == "IncrementXTVC") - ParsedCommand = RF::Command::IncrementXTVC; - else if(input_line == "DecrementXTVC") - ParsedCommand = RF::Command::DecrementXTVC; - else if(input_line == "DecrementYTVC") - ParsedCommand = RF::Command::DecrementYTVC; - else if(input_line == "Release") - ParsedCommand = RF::Command::Release; - else - ParsedCommand = RF::Command::None; - - return ParsedCommand; + return ParseCommand(input_line); } \ No newline at end of file diff --git a/hardware_test/RF.cpp b/hardware_test/RF.cpp index a42517a..6aae288 100644 --- a/hardware_test/RF.cpp +++ b/hardware_test/RF.cpp @@ -61,31 +61,5 @@ RF::Command RF::GetCommand() // Will check for commands and return the received std::getline(std::cin, input_line); std::cout << "GOT: " << input_line << "\n" << std::flush; - // if statement for which command to return - RF::Command ParsedCommand = RF::Command::None; - - if(input_line == "ABORT") - ParsedCommand = RF::Command::ABORT; - else if(input_line == "Startup") - ParsedCommand = RF::Command::Startup; - else if(input_line == "TestTVC") - ParsedCommand = RF::Command::TestTVC; - else if(input_line == "GoIdle") - ParsedCommand = RF::Command::GoIdle; - else if(input_line == "Ignite") - ParsedCommand = RF::Command::Ignite; - else if(input_line == "IncrementYTVC") - ParsedCommand = RF::Command::IncrementYTVC; - else if(input_line == "IncrementXTVC") - ParsedCommand = RF::Command::IncrementXTVC; - else if(input_line == "DecrementXTVC") - ParsedCommand = RF::Command::DecrementXTVC; - else if(input_line == "DecrementYTVC") - ParsedCommand = RF::Command::DecrementYTVC; - else if(input_line == "Release") - ParsedCommand = RF::Command::Release; - else - ParsedCommand = RF::Command::None; - - return ParsedCommand; + return ParseCommand(input_line); } \ No newline at end of file diff --git a/include/RF.hpp b/include/RF.hpp index 4fa1589..ce1c075 100644 --- a/include/RF.hpp +++ b/include/RF.hpp @@ -1,7 +1,8 @@ #include #include #include - +#include +#include #define FRAME_MAGIC_NUMBER 0xDEADBEEF // signals a new data frame, used in RF transmission #define STRING_MAGIC_NUMBER 0xBABAFACE // Signals a new string, used in RF transmission @@ -33,6 +34,40 @@ class RF { }; + ParseCommand(std::string input_line){ + // Define the regular expression to match leading and trailing whitespace/newline characters + std::regex pattern("^\\s+|\\s+$"); + + // Replace leading and trailing whitespace/newline characters with an empty string + input_line = std::regex_replace(input_line, pattern, ""); + + RF::Command ParsedCommand = RF::Command::None; + if(input_line == "ABORT") + ParsedCommand = RF::Command::ABORT; + else if(input_line == "Startup") + ParsedCommand = RF::Command::Startup; + else if(input_line == "TestTVC") + ParsedCommand = RF::Command::TestTVC; + else if(input_line == "GoIdle") + ParsedCommand = RF::Command::GoIdle; + else if(input_line == "Ignite") + ParsedCommand = RF::Command::Ignite; + else if(input_line == "IncrementYTVC") + ParsedCommand = RF::Command::IncrementYTVC; + else if(input_line == "IncrementXTVC") + ParsedCommand = RF::Command::IncrementXTVC; + else if(input_line == "DecrementXTVC") + ParsedCommand = RF::Command::DecrementXTVC; + else if(input_line == "DecrementYTVC") + ParsedCommand = RF::Command::DecrementYTVC; + else if(input_line == "Release") + ParsedCommand = RF::Command::Release; + else + ParsedCommand = RF::Command::None; + + return ParsedCommand; + } + struct rfFrame // A structure derrived from telemFrame that only contains a subset of attributes in order to save space. { uint32_t magic_number;