Merge branch 'dev' of https://github.com/Propulsive-Landing/ferda int…

…o dev
Propulsive-Landing · Oct 8, 2024 · 89be609 · 89be609
2 parents 920e992 + dd6f93f
commit 89be609
Show file tree

Hide file tree

Showing 14 changed files with 150 additions and 547 deletions.
diff --git a/Makefile b/Makefile
diff --git 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,19 +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
-        return RF::Command::None;
+    return ParseCommand(input_line);
 }
diff --git a/hardware/TVC.cpp b/hardware/TVC.cpp
@@ -7,20 +7,20 @@
 #include <MissionConstants.hpp>
 
 
-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;
 
-    servoAngle += 90 + MissionConstants::kTvcYCenterAngle;
+    servoAngle += 90 + MissionConstants::kTvcXCenterAngle;
     servoAngle = (servoAngle < 0) ? 0 : servoAngle;
     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)
+void TVC::SetTVCY(double angle_rad)
 {
 
     double degrees = angle_rad * MissionConstants::kRad2Deg;
@@ -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));
 }
diff --git a/hardware_simulation/RF.cpp b/hardware_simulation/RF.cpp
@@ -61,19 +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
-    if(input_line == "ABORT")
-        return RF::Command::ABORT;
-    else if(input_line == "Startup")
-        return RF::Command::Startup;
-    else if(input_line == "TestTVC")
-        return RF::Command::TestTVC;
-    else if(input_line == "GoIdle")
-        return RF::Command::GoIdle;
-    else if(input_line == "Ignite")
-        return RF::Command::Ignite;
-    else if(input_line == "Release")
-        return RF::Command::Release;
-    else
-        return RF::Command::None;
+    return ParseCommand(input_line);
 }
diff --git a/hardware_test/RF.cpp b/hardware_test/RF.cpp
@@ -61,15 +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
-    if(input_line == "ABORT")
-        return RF::Command::ABORT;
-    else if(input_line == "Startup")
-        return RF::Command::Startup;
-    else if(input_line == "Ignite")
-        return RF::Command::Ignite;
-    else if(input_line == "Release")
-        return RF::Command::Release;
-    else
-        return RF::Command::None;
+    return ParseCommand(input_line);
 }
diff --git a/include/Controller.hpp b/include/Controller.hpp
@@ -12,11 +12,7 @@
 
 class Controller{
 
-//Constant to be figured out later
-//int kNumberControllerGains = 10;
-
 private:
-    TVC tvc;
     std::vector<std::vector<double>> euler_queue;
     Eigen::Matrix<double, 8, 1> x_control;
     Eigen::Matrix<double, 2* MissionConstants::kNumberControllerGains, 8> controller_gains; 
@@ -27,6 +23,7 @@ class Controller{
     int current_iteration_index = 0;
 
 public:
+    TVC tvc;
     Eigen::Vector2d input;
     double loopTime = 0.005;
     Controller(TVC& tvc);   
@@ -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/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/include/Navigation.hpp b/include/Navigation.hpp
@@ -25,7 +25,7 @@ class Navigation
         void reset();
         Eigen::Matrix<double, 12, 1> GetNavigation(); // Defintion of state matrix: TODO (determine dimensions and document form)
         void UpdateNavigation(); // Defintion updates: TODO (determine dimensions and document form)
-        std::tuple<double,double,double> ComputeAngularRollingAverage();
+        std::tuple<double,double,double> ComputeAngularRollingAverage(std::vector<double> d_theta_now);
         Eigen::Matrix3d CreateRotationalMatrix(double phi, double theta, double psi);
         double GetHeight();
         std::tuple<double, double, double> GetBodyAcceleration();

diff --git a/include/RF.hpp b/include/RF.hpp
@@ -1,7 +1,8 @@
 #include <string>
 #include <fstream>
 #include <stdio.h>
-
+#include <map>
+#include <regex>
 
 #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
@@ -22,21 +23,59 @@ class RF {
             None,
             ABORT,
             Startup,
-	    TestTVC,
-	    GoIdle,
+            TestTVC,
+            GoIdle,
             Ignite,
-            Release
+            Release,
+            IncrementXTVC,
+            IncrementYTVC,
+            DecrementXTVC,
+            DecrementYTVC,
         };
 
 
+        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;
 
             uint16_t mode;
             float euler[3];
             float velocity[3];
-	    float input[2];
+	        float input[2];
             float dt;
 
             uint32_t footer;

diff --git 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); 
 };
diff --git 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<double> currentAngle {stateEstimate(6), stateEstimate(7)};
+    std::vector<double> 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/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);