diff --git a/main_controller/CMakeLists.txt b/main_controller/CMakeLists.txt
new file mode 100644
index 0000000..bad9d77
--- /dev/null
+++ b/main_controller/CMakeLists.txt
@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.0 FATAL_ERROR)
+project(MainController)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# Point to the location of libtorch
+set(CMAKE_PREFIX_PATH "/opt/torch")
+
+find_package(Torch REQUIRED)
+find_package(nlohmann_json REQUIRED)
+
+# Add the main_controller executable
+add_executable(main_controller
+    main.cpp
+    ../motors/motor_api.cpp
+    ../joint_estimator/jointEstimator.cpp
+    ../sensors/getSensorData.cpp
+    ../models/model.cpp
+    clean_angles/clean.cpp
+    torque_controller/torqueController.cpp
+    ../sensors/data_collection.cpp
+    ../sensors/liveSensorData.cpp
+    ../sensors/rpi_tca9548a.cpp
+    ../sensors/sensor_preprocessing.cpp
+    ../sensors/bno055.c
+    ../sensors/bno055.h
+)
+
+# Link required libraries, including WiringPi
+target_link_libraries(main_controller
+    "${TORCH_LIBRARIES}"
+    nlohmann_json::nlohmann_json
+    modbus
+    wiringPi
+)
+# target_include_directories(main_controller PRIVATE ../sensors)
+# Set the C++ standard for the target
+set_property(TARGET main_controller PROPERTY CXX_STANDARD 17)
diff --git a/main_controller/main.cpp b/main_controller/main.cpp
new file mode 100644
index 0000000..009f0cb
--- /dev/null
+++ b/main_controller/main.cpp
@@ -0,0 +1,277 @@
+#include <torch/script.h>
+#include <torch/torch.h>
+
+#include <chrono>
+#include <csignal>
+#include <iostream>
+#include <thread>
+#include <vector>
+#include <cmath>
+#include <atomic>
+#include "../sensors/bno055.h"
+#include "../sensors/rpi_tca9548a.h"
+
+#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"
+#include "../sensors/sensor_preprocessing.h"
+#include "../sensors/data_collection.h" 
+#include "../sensors/liveSensorData.h"   // header file for sensor preprocessing
+
+// test settings
+const bool SIMULATION = false;  // true when running without motors
+const int MOTOR_NUMBER = 0;   // number of motors 0-4
+const int SLEEP_TIME = 100;   // sleep time in ms
+std::vector<std::vector<float>> full_sensor_buffer;
+
+int16_t clampTorque(int16_t torque, int16_t min, int16_t max) {
+  return std::max(min, std::min(max, torque));
+}
+
+// Function to limit torque change per time step
+int16_t limitTorqueChange(int16_t currentTorque, int16_t previousTorque, int16_t maxChange) {
+  int16_t delta = currentTorque - previousTorque;
+  if (std::abs(delta) > maxChange) {
+    return previousTorque + (delta > 0 ? maxChange : -maxChange);
+  }
+  return currentTorque;
+}
+
+// to ensure motors shut down on Ctrl+C
+static std::vector<Motor>* g_motors_ptr = nullptr;
+volatile sig_atomic_t shutdown_requested = 0;
+// 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);
+// }
+void signalHandler(int signum) {
+  shutdown_requested = 1;
+  tca.no_channel();
+}
+
+
+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;
+  std::vector<int16_t> previous_torque_values(4, 0);
+  int addr = 0x29;
+  // std::vector<bno055_t> sensors;// Vector to store sensor objects
+
+  // 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);
+    std::signal(SIGTERM, 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";
+
+  // initialize sensors
+  std::vector<bno055_t> sensors(6);
+  rpi_tca9548a tca;
+  tca.init(0x70);
+  tca.no_channel();
+
+  for (size_t i = 0; i < sensorChannels.size(); i++) {
+    tca.set_channel(sensorChannelsBackup[i]);
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    //bno055_t imu;
+
+    if (initialize_imu(&sensors[i], addr) != BNO055_SUCCESS) {
+        std::cerr << "Failed to initialize sensor at address 0x" << std::hex << addr << std::endl;
+        continue;  // Skip this sensor and continue with others
+    }
+    if (setup_imu(&sensors[i]) != BNO055_SUCCESS) {
+        std::cerr << "Failed to set operation mode for sensor at address 0x" << std::hex << addr << std::endl;
+        continue;
+    }
+    //sensors.push_back(imu);
+  }
+  //initialize_sensors_test(sensors, tca, 0x29);
+  std::vector<double> euler_roll;
+
+  // Control system configuration/initializing
+  JointEstimator hipRight_estimator;
+  JointEstimator kneeRight_estimator;
+  JointEstimator hipLeft_estimator;
+  JointEstimator kneeLeft_estimator;
+
+
+  // Main loop
+  while (true) {
+    std::cout << "Running main loop: shutdown requested: " << shutdown_requested << std::endl;
+    if (shutdown_requested) break;
+    auto loop_start = std::chrono::steady_clock::now();
+    
+    euler_roll = liveSensorData(sensors, tca);
+    std::vector<float> euler_roll_float(euler_roll.begin(), euler_roll.end());
+    full_sensor_buffer.push_back(euler_roll_float);
+
+    if (full_sensor_buffer.size() > 100) {
+        full_sensor_buffer.erase(full_sensor_buffer.begin(), full_sensor_buffer.end() - 100);
+    }
+
+    if (full_sensor_buffer.size() < 30) {
+        std::cout << "Waiting for 30 samples...\n";
+        auto loop_end = std::chrono::steady_clock::now();
+        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(loop_end - loop_start);
+        int remaining_time = SLEEP_TIME - static_cast<int>(elapsed.count());
+        if (remaining_time > 0) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(remaining_time));
+        }
+        continue;
+    }
+    //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
+    std::vector<std::vector<float>> processed = full_sensor_buffer;
+    process_sensor_data(processed);
+
+    std::vector<std::vector<float>> input_to_model(processed.end() - 30, processed.end());
+    predicted_angles = predict_joint_angles(model, input_to_model);
+
+    // 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);
+      
+    for (size_t i = 0; i < torque_values.size(); ++i) {
+      torque_values[i] = clampTorque(torque_values[i], -500, 500);  // Clamp to [-500, 500]
+      torque_values[i] = limitTorqueChange(torque_values[i], previous_torque_values[i], 50);  // Limit change to 50
+    }
+    previous_torque_values = torque_values;
+    if (!SIMULATION) {
+      if (shutdown_requested) break;
+      for (size_t i = 0; i < motors.size(); ++i) {
+        motors[i].setTargetTorque(torque_values[i]);
+        if (shutdown_requested) break;
+        log_message("torque for motor " + std::to_string(i) + ": " +
+                    std::to_string(torque_values[i]));
+      }
+    }
+
+    auto loop_end = std::chrono::steady_clock::now();
+    auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(loop_end - loop_start);
+    int remaining_time = SLEEP_TIME - static_cast<int>(elapsed.count());
+    if (remaining_time > 0) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(remaining_time));
+    }
+  }
+  std::cout << "Disabling motors...\n";
+    for (auto& m : motors) {
+        m.disconnectMotor();
+    }
+
+    std::cout << "Shutdown complete.\n";
+    return 0;
+  }
diff --git a/model.pt b/model.pt
new file mode 100644
index 0000000..aa6cf9f
Binary files /dev/null and b/model.pt differ
diff --git a/models/model.cpp b/models/model.cpp
new file mode 100644
index 0000000..2d9b895
--- /dev/null
+++ b/models/model.cpp
@@ -0,0 +1,65 @@
+#include <torch/torch.h>
+#include <torch/script.h>
+#include <vector>
+#include <iostream>
+
+std::vector<float> mean = {
+    -0.2551588541666669,
+    -0.5622835937500013,
+    -0.6163348958333331,
+    -0.10561770833333331,
+    2.1784476562499995,
+    0.005826041666666451
+};
+
+std::vector<float> scale = {
+    12.578133062788302,
+    19.253724193808857,
+    6.668231683716051,
+    7.107068858678035,
+    15.89329980993656,
+    5.110401095770009
+};
+
+/**
+ * Predicts the next set of joint angles using the last 30 timesteps.
+ * 
+ * @param model A reference to the TorchScript model.
+ * @param last_30_timesteps A vector of 30 timesteps, each timestep being a 6-float vector.
+ *                          Shape: [30][6] = total 180 values in row-major format.
+ * @return A std::vector<float> containing 6 predicted joint angles.
+ */
+std::vector<float> predict_joint_angles(torch::jit::script::Module& model, const std::vector<std::vector<float>>& last_30_timesteps) {
+    if (last_30_timesteps.size() != 30) {
+        throw std::invalid_argument("Expected 30 timesteps");
+    }
+
+    at::Tensor input_tensor = torch::zeros({1, 30, 6});
+    for (int t = 0; t < 30; ++t) {
+        if (last_30_timesteps[t].size() != 6) {
+            throw std::invalid_argument("Each timestep must have 6 joint angles");
+        }
+        for (int j = 0; j < 6; ++j) {
+            float normalized = (last_30_timesteps[t][j] - mean[j]) / scale[j];
+            input_tensor[0][t][j] = normalized;
+        }
+    }
+
+    std::vector<torch::jit::IValue> inputs;
+    inputs.push_back(input_tensor);
+
+    at::Tensor output = model.forward(inputs).toTensor();
+
+    std::vector<float> denormalized_output(6);
+    for (int i = 0; i < 6; ++i) {
+        float val = output[0][i].item<float>();
+        denormalized_output[i] = val * scale[i] + mean[i];
+    }
+    std::cout << "Predicted joint angles: ";
+    for (const auto& angle : denormalized_output) {
+        std::cout << angle << " ";
+    }
+    std::cout << std::endl;
+
+    return denormalized_output;
+}
diff --git a/models/model.h b/models/model.h
new file mode 100644
index 0000000..86374c9
--- /dev/null
+++ b/models/model.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#include <torch/torch.h>
+#include <torch/script.h>
+#include <vector>
+
+std::vector<float> predict_joint_angles(torch::jit::script::Module& model, const std::vector<std::vector<float>>& last_30_timesteps);
diff --git a/sensors/data_collection.cpp b/sensors/data_collection.cpp
new file mode 100644
index 0000000..6b2d022
--- /dev/null
+++ b/sensors/data_collection.cpp
@@ -0,0 +1,241 @@
+#include "data_collection.h"
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <chrono>
+#include <thread>
+#include <csignal>
+#include <ctime>
+#include <string>
+#include <wiringPiI2C.h>
+#include <unistd.h>
+#include "bno055.h"
+#include "rpi_tca9548a.h"
+#include <nlohmann/json.hpp>
+
+
+using json = nlohmann::json;
+using namespace std::chrono;
+using namespace std::this_thread;
+
+std::ofstream logFile;
+bool running = true;
+rpi_tca9548a tca;
+int fd;
+
+const std::vector<int> sensorChannels = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20};
+const std::vector<int> sensorChannelsBackup = {0, 1, 2, 3, 4, 5};
+const std::vector<std::string> sensorLocations = {"Left Hip", "Left Knee", "Left Ankle", "Right Ankle","Right Knee", "Right Hip"};
+
+// void signal_handler(int signal) {
+//     std::cout << "\nStopping data logging..." << std::endl;
+//     running = false;
+// }
+
+bool initI2C() {
+    fd = wiringPiI2CSetup(0x29);
+    if (fd == -1) {
+        std::cerr << "Failed to initialize I2C connection!" << std::endl;
+        return false;
+    }
+    return true;
+}
+
+s8 I2C_bus_write_with_tca(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt) {
+    if (fd == -1) return BNO055_ERROR;
+    for (int i = 0; i < cnt; i++) {
+        if (wiringPiI2CWriteReg8(fd, reg_addr + i, reg_data[i]) == -1) {
+            return BNO055_ERROR;
+        }
+    }
+    return BNO055_SUCCESS;
+}
+
+s8 I2C_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt) {
+    if (fd == -1) return BNO055_ERROR;
+    for (int i = 0; i < cnt; i++) {
+        reg_data[i  ] = wiringPiI2CReadReg8(fd, reg_addr + i);
+        if (reg_data[i] == -1) return BNO055_ERROR;
+    }
+    return BNO055_SUCCESS;
+}
+
+s8 read_euler_angles(bno055_t* imu, bno055_euler_double_t* euler) {
+    if (bno055_convert_double_euler_hpr_deg(euler) != BNO055_SUCCESS) {
+        std::cerr << "Failed to read Euler angles!" << std::endl;
+        return BNO055_ERROR;
+    }
+    return BNO055_SUCCESS;
+}
+
+// Initialize the IMU sensor (Assign function pointers and set device address)
+s8 initialize_imu(bno055_t* imu, u8 dev_addr) {
+    imu->bus_write  = I2C_bus_write_with_tca;
+    imu->bus_read   = I2C_bus_read;
+    imu->delay_msec = delay_msec;
+    imu->dev_addr   = dev_addr;
+
+    if (bno055_init(imu) != BNO055_SUCCESS) {
+        std::cerr << "BNO055 initialization failed!" << std::endl;
+        return BNO055_ERROR;
+    }
+    return BNO055_SUCCESS;
+}
+
+// Configure the IMU operation mode
+s8 setup_imu(bno055_t* imu) {
+    if (bno055_set_operation_mode(BNO055_OPERATION_MODE_NDOF) != BNO055_SUCCESS) {
+        std::cerr << "Failed to set operation mode!" << std::endl;
+        return BNO055_ERROR;
+    }
+    delay_msec(BNO055_MODE_SWITCHING_DELAY);
+    return BNO055_SUCCESS;
+}
+
+// void initialize_sensors_test(std::vector<bno055_t> sensors, rpi_tca9548a tca , int addr){
+//     // int addr = 0x29;
+//     // std::vector<bno055_t> sensors;// Vector to store sensor objects
+//     for (size_t i = 0; i < sensorChannels.size(); i++) {
+//         tca.set_channel(sensorChannelsBackup[i]);
+//         usleep(10000);
+//         bno055_t imu;
+
+//         if (initialize_imu(&imu, addr) != BNO055_SUCCESS) {
+//             std::cerr << "Failed to initialize sensor at address 0x" << std::hex << addr << std::endl;
+//             continue;  // Skip this sensor and continue with others
+//         }
+//         if (setup_imu(&imu) != BNO055_SUCCESS) {
+//             std::cerr << "Failed to set operation mode for sensor at address 0x" << std::hex << addr << std::endl;
+//             continue;
+//         }
+//         sensors.push_back(imu);
+//     }
+// }
+
+void delay_msec(u32 msec) {
+    usleep(msec * 1000);
+}
+
+std::string get_human_readable_timestamp() {
+    auto now = system_clock::now();
+    auto ms = duration_cast<milliseconds>(now.time_since_epoch()) % 1000;
+    std::time_t now_c = system_clock::to_time_t(now);
+    std::tm local_tm = *std::localtime(&now_c);
+    char buffer[20];
+    std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", &local_tm);
+    return std::string(buffer) + "." + std::to_string(ms.count());
+}
+
+// int main(int argc, char* argv[]) {
+//     signal(SIGINT, signal_handler);
+//     if (!initI2C()) return -1;
+//     tca.init(0x70);
+    
+//     std::string filename;
+//     if (argc > 1) {
+//         filename = argv[1];  // Take filename from command-line argument
+//     } else {
+//         std::cout << "Enter filename for logging (e.g., imu_data.json): ";
+//         std::cin >> filename;
+//     }
+
+//     logFile.open(filename, std::ios::app);
+//     if (!logFile.is_open()) {
+//         std::cerr << "Failed to open imu_data.json for writing" << std::endl;
+//         return -1;
+//     }
+    
+//     std::vector<bno055_t> sensors(5);
+//     for (size_t i = 0; i < sensorChannels.size(); i++) {
+//         tca.set_channel(sensorChannelsBackup[i]);
+//         usleep(10000);
+
+//         initialize_imu(sensors[i], 0x29);
+//         setup_imu(sensors[i]);
+//     }
+
+//     int sampleIntervalMs = 2000;
+//     while (running) {
+//         auto start_time = steady_clock::now();
+        
+//         auto t1 = steady_clock::now();
+//         json logEntry;
+//         logEntry["timestamp"] = get_human_readable_timestamp();
+//         logEntry["sensors"] = json::array();
+//         auto t2 = steady_clock::now();
+
+//         for (size_t i = 0; i < sensors.size(); i++) {
+//             auto sensor_start = steady_clock::now();
+            
+//             tca.set_channel(sensorChannelsBackup[i]);
+//             auto t3 = steady_clock::now();
+            
+//             bno055_euler_double_t euler;
+//             bno055_accel_double_t accel;
+//             bno055_linear_accel_double_t linear_accel;
+//             bno055_gravity_double_t gravity;
+//             bno055_gyro_double_t angular_velocity;
+//             bno055_mag_double_t mag_field;
+//             bno055_quaternion_t quater;
+
+//             if (bno055_convert_double_euler_hpr_deg(&euler) != BNO055_SUCCESS) {
+//                 std::cerr << "Failed to read euler from: " << sensorLocations[i] << std::endl;
+//             }
+//             std::cout << sensorLocations[i] << ": " << euler.h << " Roll: " << euler.r << " Pitch: " << euler.p << std::endl;
+//             if (euler.h == 0.0 && euler.r == 0.0 && euler.p == 0.0){
+//                 std::cerr << "BAD READING ALL ZEROS " << std::endl;
+//             }
+//             //bno055_convert_double_accel_xyz_msq(&accel);
+//             //bno055_convert_double_linear_accel_xyz_msq(&linear_accel);
+//             //bno055_convert_double_gravity_xyz_msq(&gravity);
+//             //bno055_convert_double_gyro_xyz_dps(&angular_velocity);
+//             //bno055_convert_double_mag_xyz_uT(&mag_field);
+//             //bno055_read_quaternion_wxyz(&quater);
+
+//             auto t4 = steady_clock::now();
+
+//             json sensorData;
+//             sensorData["location"] = sensorLocations[i];
+//             sensorData["euler"] = {{"heading", euler.h}, {"roll", euler.r}, {"pitch", euler.p}};
+//             // sensorData["acceleration"] = {{"x", accel.x}, {"y", accel.y}, {"z", accel.z}};
+//             // sensorData["linear_acceleration"] = {{"x", linear_accel.x}, {"y", linear_accel.y}, {"z", linear_accel.z}};
+//             // sensorData["gravity"] = {{"x", gravity.x}, {"y", gravity.y}, {"z", gravity.z}};
+//             // sensorData["angular_velocity"] = {{"x", angular_velocity.x}, {"y", angular_velocity.y}, {"z", angular_velocity.z}};
+//             // sensorData["magnetic_field"] = {{"x", mag_field.x}, {"y", mag_field.y}, {"z", mag_field.z}};
+//             //sensorData["quaternion"] = {{"w", quater.w}, {"x", quater.x}, {"y", quater.y}, {"z", quater.z}};
+            
+//             logEntry["sensors"].push_back(sensorData);
+            
+//             auto sensor_end = steady_clock::now();
+
+//             // std::cout << "Sensor " << i << " switch time: " 
+//             //         << duration_cast<milliseconds>(t3 - sensor_start).count() << " ms" << std::endl;
+
+//             std::cout << "Sensor " << i << " read time: " 
+//                     << duration_cast<milliseconds>(t4 - t3).count() << " ms" << std::endl;
+
+//             // std::cout << "Sensor " << i << " JSON prep time: " 
+//             //         << duration_cast<milliseconds>(sensor_end - t4).count() << " ms" << std::endl;
+//         }
+
+//         auto t5 = steady_clock::now();
+//         logFile << logEntry.dump() << std::endl;
+//         logFile.flush();
+//         auto t6 = steady_clock::now();
+
+//         // std::cout << "File write & flush time: " 
+//         //         << duration_cast<milliseconds>(t6 - t5).count() << " ms" << std::endl;
+
+//         auto end_time = steady_clock::now();
+//         // std::cout << "Total loop time: " 
+//         //         << duration_cast<milliseconds>(end_time - start_time).count() << " ms" << std::endl;
+
+//         milliseconds sleep_time = milliseconds(sampleIntervalMs) - duration_cast<milliseconds>(end_time - start_time);
+//         if (sleep_time.count() > 0) sleep_for(sleep_time);
+//         std::cout << " ------------- " << std::endl;
+//     }
+
+//     logFile.close();
+
+//     return 0;
+// }
diff --git a/sensors/data_collection.h b/sensors/data_collection.h
new file mode 100644
index 0000000..9d67e0e
--- /dev/null
+++ b/sensors/data_collection.h
@@ -0,0 +1,43 @@
+#ifndef DATA_COLLECTION_H
+#define DATA_COLLECTION_H
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <chrono>
+#include <thread>
+#include <csignal>
+#include <ctime>
+#include <string>
+#include <wiringPiI2C.h>
+#include <unistd.h>
+#include <nlohmann/json.hpp>
+#include "bno055.h"
+#include "rpi_tca9548a.h"
+
+using json = nlohmann::json;
+using namespace std::chrono;
+using namespace std::this_thread;
+
+// Globals
+extern std::ofstream logFile;
+extern bool running;
+extern rpi_tca9548a tca;
+extern int fd;
+
+extern const std::vector<int> sensorChannels;
+extern const std::vector<int> sensorChannelsBackup;
+extern const std::vector<std::string> sensorLocations;
+
+// Functions
+bool initI2C();
+s8 I2C_bus_write_with_tca(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt);
+s8 I2C_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt);
+s8 read_euler_angles(bno055_t* imu, bno055_euler_double_t* euler);
+s8 initialize_imu(bno055_t* imu, u8 dev_addr);
+s8 setup_imu(bno055_t* imu);
+//void initialize_sensors_test(std::vector<bno055_t>& sensors, rpi_tca9548a& tca, int addr);
+void delay_msec(u32 msec);
+std::string get_human_readable_timestamp();
+
+#endif // DATA_COLLECTION_H
diff --git a/sensors/getSensorData.cpp b/sensors/getSensorData.cpp
new file mode 100644
index 0000000..0866e6d
--- /dev/null
+++ b/sensors/getSensorData.cpp
@@ -0,0 +1,55 @@
+#include "getSensorData.h"
+
+
+using json = nlohmann::json;
+
+
+std::vector<std::vector<float>> get_sensor_data(
+    const std::string &filename,
+    int chunk_index,
+    int step_size
+){ 
+    std::ifstream file(filename);
+    std::string line;
+    std::vector<std::vector<float>> output;
+    
+    if (!file.is_open()) {
+        std::cerr << "Failed to open file: " << filename << std::endl;
+        return {};
+    }
+
+    int start_line = chunk_index;
+    int end_line = start_line + step_size;
+    int current_line = 0;
+
+    while (std::getline(file, line)) {
+        if (current_line >= start_line && current_line < end_line) {
+            json entry = json::parse(line);
+
+            std::array<float, 6> rolls;
+            int idx = 0;
+
+            for (const auto &sensor : entry["sensors"]) {
+                rolls[idx++] = sensor["euler"]["roll"];
+            }
+
+            if (idx == 6) output.push_back(std::vector<float>(rolls.begin(), rolls.end()));
+        }
+
+        current_line++;
+        if (current_line >= end_line) break;
+    }
+    return output;
+}
+
+// int main(){
+//     auto chunk = get_sensor_data("/home/dylan-exo/control_system/filtered_imu_data_treadmill_5min_1.9mph.json", 0, 30);
+
+//     for (size_t i = 0; i < chunk.size(); ++i) {
+//         std::cout << "step " << i << ": ";
+//         for (float roll : chunk[i]) {
+//             std::cout << roll << " ";
+//         }
+//         std::cout << std::endl;
+//     }
+// }
\ No newline at end of file
diff --git a/sensors/getSensorData.h b/sensors/getSensorData.h
new file mode 100644
index 0000000..0ce7b17
--- /dev/null
+++ b/sensors/getSensorData.h
@@ -0,0 +1,17 @@
+#include <iostream>
+#include <fstream>
+#include <nlohmann/json.hpp>
+#include <vector>
+#include <string>
+#include <array>
+
+
+/**
+ * Parses roll angles from json .
+ * 
+ * @param filename Path to JSON file.
+ * @param step_size Number of timesteps per chunk (i.e. read 30 lines per call).
+ * @param chunk_index Index of the chunk to extract (index 1:0-30 entries. index 2: 31-60).
+ * @return A vector of [step_size][6]
+ */
+std::vector<std::vector<float>> get_sensor_data(const std::string &filename, int chunk_index, int step_size);
\ No newline at end of file
diff --git a/sensors/liveSensorData.cpp b/sensors/liveSensorData.cpp
new file mode 100644
index 0000000..18cb698
--- /dev/null
+++ b/sensors/liveSensorData.cpp
@@ -0,0 +1,57 @@
+#include "liveSensorData.h"
+#include "data_collection.h"
+#include "bno055.h"
+#include "rpi_tca9548a.h"
+
+// const int maxBufferSize = 30;
+
+
+
+std::vector<double> liveSensorData(std::vector<bno055_t>& sensors, rpi_tca9548a& tca) {
+    std::vector<double> roll_values;
+
+    if (!initI2C()) return {};
+
+    std::string filename = "imu_data.json";
+    logFile.open(filename, std::ios::app);
+    if (!logFile.is_open()) {
+        std::cerr << "Failed to open imu_data.json for writing" << std::endl;
+        return {};
+    }
+
+    json logEntry;
+    logEntry["timestamp"] = get_human_readable_timestamp();
+    logEntry["sensors"] = json::array();
+
+    for (size_t i = 0; i < sensors.size(); i++) {
+        bno055_euler_double_t euler;
+
+        tca.set_channel(sensorChannelsBackup[i]);
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+        if (bno055_convert_double_euler_hpr_deg(&euler) != BNO055_SUCCESS) {
+            std::cerr << "Failed to read euler from: " << sensorLocations[i] << std::endl;
+            roll_values.push_back(0.0);
+            continue;
+        }
+
+        std::cout << sensorLocations[i] << ": "
+                  << "Heading: " << euler.h
+                  << ", Roll: " << euler.r
+                  << ", Pitch: " << euler.p << std::endl;
+
+        json sensorData;
+        sensorData["location"] = sensorLocations[i];
+        sensorData["euler"] = {{"heading", euler.h}, {"roll", euler.r}, {"pitch", euler.p}};
+        logEntry["sensors"].push_back(sensorData);
+
+        roll_values.push_back(euler.r);
+    }
+
+    logFile << logEntry.dump() << std::endl;
+    logFile.flush();
+    logFile.close();
+
+    return roll_values;
+}
+
diff --git a/sensors/liveSensorData.h b/sensors/liveSensorData.h
new file mode 100644
index 0000000..72d2a41
--- /dev/null
+++ b/sensors/liveSensorData.h
@@ -0,0 +1,12 @@
+#ifndef LIVE_SENSOR_DATA_H
+#define LIVE_SENSOR_DATA_H
+
+#include "bno055.h"
+#include "rpi_tca9548a.h"
+#include <vector>
+#include <cmath>
+
+
+std::vector<double> liveSensorData(std::vector<bno055_t>& sensors, rpi_tca9548a& tca);
+
+#endif // LIVE_SENSOR_DATA_H
diff --git a/sensors/sensor_preprocessing.cpp b/sensors/sensor_preprocessing.cpp
new file mode 100644
index 0000000..aabe5fe
--- /dev/null
+++ b/sensors/sensor_preprocessing.cpp
@@ -0,0 +1,69 @@
+#include "sensor_preprocessing.h"
+#include <deque>
+#include <numeric>
+#include <cmath>
+
+static void fix_flip(std::vector<float>& signal, float diff_max, int section_size) {
+    if (signal.empty()) return;
+    std::vector<float> new_data;
+    new_data.reserve(signal.size());
+    new_data.push_back(signal[0]);
+
+    for (size_t i = 1; i < signal.size(); ++i) {
+        int start = std::max(static_cast<int>(i) - section_size, 0);
+        float mean_recent = std::accumulate(new_data.begin() + start, new_data.end(), 0.0f) / (i - start);
+        float diff = signal[i] - mean_recent;
+
+        if (diff > diff_max)
+            new_data.push_back(signal[i] - 360.0f);
+        else if (diff < -diff_max)
+            new_data.push_back(signal[i] + 360.0f);
+        else
+            new_data.push_back(signal[i]);
+    }
+
+    signal = std::move(new_data);
+}
+
+static void normalize(std::vector<float>& signal, int section_size) {
+    if (signal.empty()) return;
+
+    std::deque<float> window;
+    float total = 0.0f;
+    std::vector<float> normalized;
+    normalized.reserve(signal.size());
+
+    for (size_t i = 0; i < signal.size(); ++i) {
+        if (window.size() >= section_size) {
+            total -= window.front();
+            window.pop_front();
+        }
+        window.push_back(signal[i]);
+        total += signal[i];
+        float mean = total / static_cast<float>(window.size());
+        normalized.push_back(signal[i] - mean);
+    }
+
+    signal = std::move(normalized);
+}
+
+void process_sensor_data(std::vector<std::vector<float>>& data, int section_size, float diff_max) {
+    if (data.empty() || data[0].size() != 6) return;
+
+    // Transpose data to [6][timesteps]
+    std::vector<std::vector<float>> channels(6, std::vector<float>(data.size()));
+    for (size_t t = 0; t < data.size(); ++t)
+        for (size_t j = 0; j < 6; ++j)
+            channels[j][t] = data[t][j];
+
+    // Process each channel
+    for (int j = 0; j < 6; ++j) {
+        fix_flip(channels[j], diff_max, section_size);
+        normalize(channels[j], section_size);
+    }
+
+    // Transpose back to [timesteps][6]
+    for (size_t t = 0; t < data.size(); ++t)
+        for (size_t j = 0; j < 6; ++j)
+            data[t][j] = channels[j][t];
+}
diff --git a/sensors/sensor_preprocessing.h b/sensors/sensor_preprocessing.h
new file mode 100644
index 0000000..c6554fb
--- /dev/null
+++ b/sensors/sensor_preprocessing.h
@@ -0,0 +1,6 @@
+#pragma once
+#include <vector>
+
+// Input: 2D vector of sensor data with shape [timesteps][6]
+// Output: processed in-place
+void process_sensor_data(std::vector<std::vector<float>>& data, int section_size = 100, float diff_max = 200.0f);