feature(control system): add main_controller

control/main_controller/clean_angles/clean.cpp

@@ -0,0 +1,24 @@
+#include "clean.h"
+
+#include <vector>
+
+// Constants for knee and hip offsets
+constexpr double KNEE_OFFSET = 0;  // Example value
+constexpr double HIP_OFFSET = 0;   // Example value
+
+// Function to clean the offset in AI data readings
+std::vector<float> cleanAIOffsets(const std::vector<float>& imuReadings) {
+  std::vector<float> cleanedReadings(6);
+
+      cleanedReadings[0] = imuReadings[0] - KNEE_OFFSET; // lk
+      cleanedReadings[4] = imuReadings[4] - KNEE_OFFSET; // rk
+
+      cleanedReadings[3] = imuReadings[3] - HIP_OFFSET; // rh
+      cleanedReadings[5] = imuReadings[5] - HIP_OFFSET; // lh
+
+  // Fixing Knee angles to be relative to the hip
+  cleanedReadings[0] = cleanedReadings[0] - cleanedReadings[5]; // left leg
+  cleanedReadings[4] = cleanedReadings[4] - cleanedReadings[3]; // right leg
+
+  return cleanedReadings;
+}

control/main_controller/clean_angles/clean.h

@@ -0,0 +1,9 @@
+#ifndef CLEAN_H
+#define CLEAN_H
+
+#include <vector>
+
+// Function to clean the offset in AI data readings
+std::vector<float> cleanAIOffsets(const std::vector<float>& imuReadings);
+
+#endif  // CLEAN_H

control/main_controller/joint_estimator/jointEstimator.cpp

@@ -0,0 +1,36 @@
+#include "jointEstimator.h"
+
+#include <iostream>
+#include <vector>
+
+#include "../torque_controller/torqueController.h"
+
+// constructor with initialized values
+JointEstimator::JointEstimator()
+  : prev_angle(0.0), prev_velocity(0.0), velocity(0.0), acceleration(0.0) {}
+
+  TorqueController::JointState JointEstimator::update(double current_angle, double dt) {
+    TorqueController::JointState angle_values;
+
+    // Optional: Apply a low-pass filter to smooth the angle
+    angle_values.angle = current_angle;
+
+    // Calculate velocity
+    angle_values.velocity = (current_angle - prev_angle) / dt;
+    velocity = angle_values.velocity;
+
+    // Calculate acceleration
+    angle_values.acceleration = (velocity - prev_velocity) / dt;
+
+    // Debugging output
+    std::cout << "current_angle: " << angle_values.angle
+              << ", velocity: " << angle_values.velocity
+              << ", acceleration: " << angle_values.acceleration
+              << std::endl;
+
+    // Update previous values
+    prev_angle = current_angle;
+    prev_velocity = velocity;
+
+    return angle_values;
+  }

control/main_controller/joint_estimator/jointEstimator.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <vector>
+
+
+#include "../torque_controller/torqueController.h"
+
+class JointEstimator {
+ public:
+  JointEstimator();
+  TorqueController::JointState update(double current_angle, double dt);
+
+ private:
+  double prev_angle;
+  double prev_velocity;
+  double velocity;
+  double acceleration;
+};

control/main_controller/main.cpp

@@ -0,0 +1,177 @@
+#include <torch/script.h>
+#include <torch/torch.h>
+
+#include <chrono>
+#include <csignal>
+#include <iostream>
+#include <thread>
+#include <vector>
+#include <cmath>
+
+#include "../models/model.h"           // header file for model
+#include "../motors/motor_api.h"       // header file for motor API
+#include "../sensors/getSensorData.h"  // header file for normzalied sensor data
+#include "clean_angles/clean.h"        // header file for clean angles
+#include "joint_estimator/jointEstimator.h"
+#include "torque_controller/torqueController.h"
+
+// test settings
+const bool SIMULATION = true;  // true when running without motors
+const int MOTOR_NUMBER = 0;   // number of motors 0-4
+const int SLEEP_TIME = 100;   // sleep time in ms
+
+// to ensure motors shut down on Ctrl+C
+static std::vector<Motor>* g_motors_ptr = nullptr;
+void signalHandler(int signum) {
+  std::cout << "\nCaught signal " << signum << ", disabling motors...\n";
+  if (g_motors_ptr) {
+    for (auto& m : *g_motors_ptr) {
+      m.stopMotor();
+      m.disconnectMotor();
+    }
+  }
+  std::exit(signum);
+}
+
+std::string get_current_timestamp() {
+  auto now = std::time(nullptr);
+  char buf[100];
+  strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S", localtime(&now));
+  return std::string(buf);
+}
+
+void log_message(const std::string& message) {
+  std::cout << get_current_timestamp() << " - " << message << std::endl;
+}
+
+int main() {
+  std::vector<Motor> motors;  // vector of motors
+  double dt = 0.1;            // Sampling rate for model
+  int chunk_index = 0;
+
+  // Load the model
+  // load the models weights into memory
+  torch::jit::script::Module model;
+  try {
+    model = torch::jit::load("../model.pt");
+    model.eval();
+    log_message("Model loaded successfully.");
+    std::cout <<  std::endl;
+  } catch (const c10::Error& e) {
+    log_message("Error loading the model: " + std::string(e.what()));
+    return -1;
+  }
+
+  // Initialize motors
+  if (!SIMULATION) {
+    motors = {
+        Motor("/dev/ttyUSB0", 1),
+        // Motor("/dev/ttyUSB1", 2),
+        // Motor("/dev/ttyUSB2", 3),
+        // Motor("/dev/ttyUSB3", 4),
+    };
+
+    g_motors_ptr = &motors;
+
+    // register Ctrl+C handler
+    std::signal(SIGINT, signalHandler);
+
+    for (auto& m : motors) {
+      if (!m.initializeMotor()) {
+        std::cerr << "Failed to init motor (slave " << m.getStatus() << ")\n";
+        return -1;
+      }
+      m.setOperationMode(PT_MODE);
+      m.setMaxTorque(1000);
+    }
+  }
+
+  // initialize variables
+  constexpr size_t WINDOW = 30;
+  std::vector<std::vector<float>> sensor_history(6, std::vector<float>(WINDOW));
+  TorqueController::JointState hipLeft, kneeLeft, hipRight,
+      kneeRight;  // initializing joints
+  std::vector<float> predicted_angles(6), clean_angles(6);
+  std::vector<int16_t> torque_values(4);
+  const std::string sensor_file =
+      "../filtered_imu_data_treadmill_5min_1.9mph.json";
+
+  // Control system configuration/initializing
+  JointEstimator hipRight_estimator;
+  JointEstimator kneeRight_estimator;
+  JointEstimator hipLeft_estimator;
+  JointEstimator kneeLeft_estimator;
+
+  // Main loop
+  while (true) {
+    auto raw = get_sensor_data(sensor_file, chunk_index, WINDOW);
+    if (raw.size() < WINDOW) {
+      std::cerr << "Only " << raw.size() << " samples available; need "
+                << WINDOW << "\n";
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      continue;
+    }
+
+    chunk_index += 1;
+
+    // take the last WINDOW entries and transpose into [6][30]
+    // for (size_t j = 0; j < 6; ++j) {
+    //   for (size_t t = 0; t < WINDOW; ++t) {
+    //     sensor_history[j][t] = raw[t][j];
+    //   }
+    // }
+
+    // lk la ra rh rk lh
+
+    // running AI model
+    predicted_angles = predict_joint_angles(model, raw);
+
+    // Print predicted angles
+    // std::cout << "Predicted Angles:" << std::endl;
+    // for (size_t i = 0; i < predicted_angles.size(); ++i) {
+    //   std::cout << "  Joint " << i << ": " << predicted_angles[i] <<
+    //   std::endl;
+    // }
+
+    // clean function call
+    clean_angles = cleanAIOffsets(predicted_angles);
+    std::transform(clean_angles.begin(), clean_angles.end(), clean_angles.begin(),
+               [](float angle) { return angle * M_PI / 180.0f; });
+
+    // Assign angles to the corresponding joints and log the updates
+    hipRight = hipRight_estimator.update(clean_angles[3], dt);
+    std::cout << "Updated HipRight: angle = " << hipRight.angle 
+              << ", velocity = " << hipRight.velocity 
+              << ", acceleration = " << hipRight.acceleration << std::endl;
+
+    kneeRight = kneeRight_estimator.update(clean_angles[4], dt);
+    std::cout << "Updated KneeRight: angle = " << kneeRight.angle 
+              << ", velocity = " << kneeRight.velocity 
+              << ", acceleration = " << kneeRight.acceleration << std::endl;
+
+    hipLeft = hipLeft_estimator.update(clean_angles[5], dt);
+    std::cout << "Updated HipLeft: angle = " << hipLeft.angle 
+              << ", velocity = " << hipLeft.velocity 
+              << ", acceleration = " << hipLeft.acceleration << std::endl;
+
+    kneeLeft = kneeLeft_estimator.update(clean_angles[0], dt);
+    std::cout << "Updated KneeLeft: angle = " << kneeLeft.angle 
+              << ", velocity = " << kneeLeft.velocity 
+              << ", acceleration = " << kneeLeft.acceleration << std::endl;
+
+    // torque calculator function call
+    TorqueController torqueController;
+    torque_values =
+        torqueController.computeTorque(hipRight, hipLeft, kneeRight, kneeLeft);
+
+    if (!SIMULATION) {
+      for (size_t i = 0; i < motors.size(); ++i) {
+        motors[i].setTargetTorque(torque_values[i]);
+        log_message("torque for motor " + std::to_string(i) + ": " +
+                    std::to_string(torque_values[i]));
+      }
+    }
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(SLEEP_TIME));
+  }
+}