Control systems

control/FSM_general_controller/.vscode/settings.json

@@ -0,0 +1,46 @@
+{
+  "files.associations": {
+    "atomic": "cpp",
+    "bit": "cpp",
+    "cctype": "cpp",
+    "clocale": "cpp",
+    "cmath": "cpp",
+    "compare": "cpp",
+    "concepts": "cpp",
+    "cstddef": "cpp",
+    "cstdint": "cpp",
+    "cstdio": "cpp",
+    "cstdlib": "cpp",
+    "cstring": "cpp",
+    "ctime": "cpp",
+    "cwchar": "cpp",
+    "exception": "cpp",
+    "initializer_list": "cpp",
+    "ios": "cpp",
+    "iosfwd": "cpp",
+    "iostream": "cpp",
+    "istream": "cpp",
+    "iterator": "cpp",
+    "limits": "cpp",
+    "memory": "cpp",
+    "new": "cpp",
+    "ostream": "cpp",
+    "stdexcept": "cpp",
+    "streambuf": "cpp",
+    "system_error": "cpp",
+    "tuple": "cpp",
+    "type_traits": "cpp",
+    "typeinfo": "cpp",
+    "utility": "cpp",
+    "xfacet": "cpp",
+    "xiosbase": "cpp",
+    "xlocale": "cpp",
+    "xlocinfo": "cpp",
+    "xlocnum": "cpp",
+    "xmemory": "cpp",
+    "xstddef": "cpp",
+    "xstring": "cpp",
+    "xtr1common": "cpp",
+    "xutility": "cpp"
+  }
+}

control/FSM_general_controller/ActiveState.cpp

@@ -0,0 +1,130 @@
+#include "ActiveState.h"
+#include <iostream>
+#include "../Joint_Estimator/JointEstimator.h"
+#include "../Torque_Controller/TorqueController.h"
+#include "../IMU_DataLoader/IMUDataLoader.h"
+using namespace IMUDataLoader;
+
+#include <iostream>
+#include <cmath>
+#include <chrono>
+#include <thread>
+#include <fstream>
+
+// Constructor
+ActiveState::ActiveState()
+{
+}
+
+// Destructor
+ActiveState::~ActiveState()
+{
+}
+
+void ActiveState::enter()
+{
+  cout << "\nNow in Active State \n";
+}
+
+void ActiveState::update()
+{
+
+    // Setup parameters (TODO: move these parameter definitions to another file)
+    TorqueController::ModelParameters params = {
+        .m1 = 5.65, .m2 = 3.48,
+        .L1 = 0.41, .L2 = 0.4879,
+        .r1 = 0.17, .r2 = 0.1892,
+        .I1 = 0.0648, .I2 = 0.0107,
+        .g = 9.81
+    };
+
+    TorqueController controller(params);
+
+    // Load IMU Data
+    JointData knee_data = loadJointData("joint_data.json", "Right Knee");
+    const std::vector<double>& timestamps = knee_data.timestamps;
+    const std::vector<double>& knee_angles = knee_data.angles_rad;
+
+    // These variables are only for testing purposes
+    // We want to see how the coriolis and gravity term affect the torque output
+    double hip_m_acc, hip_c_vel, hip_g;
+    double knee_m_acc, knee_c_vel, knee_g;
+
+    JointEstimator hip_estimator;
+    JointEstimator knee_estimator;
+
+    hip_estimator.prime((10.0 * M_PI / 180.0) * std::sin(2 * M_PI * 1.0 * timestamps[0]));
+    knee_estimator.prime(knee_angles[0]);
+
+    //Setup logging
+    std::ofstream log("Testing/log_output.csv");
+    log << "time,hip_angle,hip_velocity,hip_acceleration,"
+       "hip_m_acc,hip_c_vel,hip_g,hip_torque,"
+       "knee_angle,knee_velocity,knee_acceleration,"
+       "knee_m_acc,knee_c_vel,knee_g,knee_torque\n";
+    /////////////////////////////////////////////////////////////////////////////
+
+    for (size_t i = 1; i < knee_angles.size(); ++i) 
+    {
+        double t = timestamps[i];
+        double dt = timestamps[i] - timestamps[i - 1];
+
+        // Run simulation for 10 seconds for testing purposes (TODO: maybe create a non-blocking keyboard interrupt to change states)
+        if (t >= 120.0) {
+            std::cout << "Finished 120-second test.\n";
+            leave();
+            current = idle;
+            return;
+        }
+
+        double hip_angle = (10.0 * M_PI / 180.0) * std::sin(2 * M_PI * 1.0 * t);
+        double knee_angle = knee_angles[i];
+
+        // Simulated joint angles (TODO: replace with AI later)
+        // make sure angle is in RADIANS!!
+        // double hip_angle = (10.0 * M_PI / 180.0) * std::sin(2 * M_PI * 1.0 * t);
+        // double knee_angle = (20.0 * M_PI / 180.0) * std::sin(2 * M_PI * 1.0 * t + M_PI / 4);
+
+        // Update speed, acceleration estimator
+        hip_estimator.update(hip_angle, dt);
+        knee_estimator.update(knee_angle, dt);
+
+        TorqueController::JointState hip = {
+            .angle = hip_angle,
+            .velocity = hip_estimator.getVelocity(),
+            .acceleration = hip_estimator.getAcceleration()
+        };
+
+        TorqueController::JointState knee = {
+            .angle = knee_angle,
+            .velocity = knee_estimator.getVelocity(),
+            .acceleration = knee_estimator.getAcceleration()
+        };
+
+        // Compute torque
+        double hip_torque = 0.0, knee_torque = 0.0;
+        controller.computeTorque(
+            hip, knee,
+            hip_torque, knee_torque,
+            hip_m_acc, hip_c_vel, hip_g,
+            knee_m_acc, knee_c_vel, knee_g
+        );
+
+        // Output to console for now (TODO: replace with motor command)
+        std::cout << "Time: " << t << "\tHip Torque: " << hip_torque << "\tKnee Torque: " << knee_torque << "\n";
+
+        // Log to CSV
+        log << t << ","
+            << hip.angle << "," << hip.velocity << "," << hip.acceleration << ","
+            << hip_m_acc << "," << hip_c_vel << "," << hip_g << "," << hip_torque << ","
+            << knee.angle << "," << knee.velocity << "," << knee.acceleration << ","
+            << knee_m_acc << "," << knee_c_vel << "," << knee_g << "," << knee_torque << "\n";
+
+        std::this_thread::sleep_for(std::chrono::duration<double>(dt));
+    }
+}
+
+void ActiveState::leave()
+{
+  cout << "\nleaving Active State\n";
+}

control/FSM_general_controller/ActiveState.h

@@ -0,0 +1,17 @@
+#ifndef ACTIVE_STATE_H
+#define ACTIVE_STATE_H
+
+#include "State.h"
+
+class ActiveState : public State
+{
+
+public:
+  ActiveState();
+  virtual ~ActiveState();
+  void enter();
+  void leave();
+  void update();
+};
+
+#endif

control/FSM_general_controller/ErrorState.cpp

@@ -0,0 +1,48 @@
+#include "ErrorState.h"
+
+ErrorState::ErrorState()
+{
+}
+
+ErrorState::~ErrorState()
+{
+}
+
+void ErrorState::enter()
+{
+  cout << "\nNow in Error State \n";
+}
+
+void ErrorState::update()
+{
+  // loop
+  while (true)
+  {
+    cout << "1. active\n2. idle \n3. exit\n";
+    int sensorReading;
+    cin >> sensorReading; // replace with sensor reading(s)
+    switch (sensorReading)
+    {
+
+      // replace cases with conditionals
+
+    case 1:
+      leave();
+      current = active;
+      return;
+
+    case 2:
+      leave();
+      current = idle;
+      return;
+
+    case 3:
+      exit(0); // exit program with 0 status (immediate shut down case)
+    }
+  }
+}
+
+void ErrorState::leave()
+{
+  cout << "\nleaving Error State \n";
+}

control/FSM_general_controller/ErrorState.h

@@ -0,0 +1,17 @@
+#ifndef ERROR_STATE_H
+#define ERROR_STATE_H
+
+#include "State.h"
+
+class ErrorState : public State
+{
+
+public:
+  ErrorState();
+  virtual ~ErrorState();
+  void enter();
+  void leave();
+  void update();
+};
+
+#endif

control/FSM_general_controller/IdleState.cpp

@@ -0,0 +1,48 @@
+#include "IdleState.h"
+
+IdleState::IdleState()
+{
+}
+
+IdleState::~IdleState()
+{
+}
+
+void IdleState::enter()
+{
+  cout << "\nNow in Idle State \n";
+}
+
+void IdleState::update()
+{
+  while (true)
+  {
+
+    cout << "1. active\n2. error \n3. exit\n";
+    int num;
+    cin >> num; // replace with sensor reading(s)
+    switch (num)
+    {
+
+      // replace cases with conditionals
+
+    case 1:
+      leave();
+      current = active;
+      return;
+
+    case 2:
+      leave();
+      current = error;
+      return;
+
+    case 3:
+      exit(0); // exit program with 0 status (fatal subModule failure)
+    }
+  }
+}
+
+void IdleState::leave()
+{
+  cout << "\nleaving Idle State\n";
+}

control/FSM_general_controller/IdleState.h

@@ -0,0 +1,17 @@
+#ifndef IDLE_STATE_H
+#define IDLE_STATE_H
+
+#include "State.h"
+
+class IdleState : public State
+{
+
+public:
+  IdleState();
+  virtual ~IdleState();
+  void enter();
+  void leave();
+  void update();
+};
+
+#endif

control/FSM_general_controller/Sensor.cpp

@@ -0,0 +1,26 @@
+
+#include "Sensor.h"
+
+double Sensor::max_sensor_reading = 0.0; // Initial value for max sensor reading
+double Sensor::min_sensor_reading = 0.0; // Initial value for min sensor reading
+
+void Sensor::setMaxSensorReading(double maxReading)
+{
+  max_sensor_reading = maxReading;
+}
+
+void Sensor::setMinSensorReading(double minReading)
+{
+  min_sensor_reading = minReading;
+}
+
+// Getters
+double Sensor::getMaxSensorReading()
+{
+  return max_sensor_reading;
+}
+
+double Sensor::getMinSensorReading()
+{
+  return min_sensor_reading;
+}

control/FSM_general_controller/Sensor.h

@@ -0,0 +1,21 @@
+#ifndef SENSOR_H
+#define SENSOR_H
+
+class Sensor
+{
+public:
+  static double max_sensor_reading;
+  static double min_sensor_reading;
+
+  // Set the max and min sensor readings
+  static void setMaxSensorReading(double maxReading);
+
+  static void setMinSensorReading(double minReading);
+
+  // Getters
+  static double getMaxSensorReading();
+
+  static double getMinSensorReading();
+};
+
+#endif