diff --git a/.gitignore b/.gitignore
index 259148f..0e3f7f3 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,32 +1,62 @@
-# Prerequisites
-*.d
+# ------------------------------------------------------------------------------
+# Build artifacts
+# ------------------------------------------------------------------------------
-# Compiled Object files
-*.slo
-*.lo
+# CMake build directory
+build/
+CMakeFiles/
+CMakeCache.txt
+cmake_install.cmake
+Makefile
+
+# Object and output files
*.o
*.obj
+*.so
+*.a
+*.dll
+*.dylib
+*.lib
+*.out
+*.exe
+*.app
+*.slo
+*.lo
+*.lai
+*.la
-# Precompiled Headers
+# Precompiled headers
*.gch
*.pch
-# Compiled Dynamic libraries
-*.so
-*.dylib
-*.dll
-
-# Fortran module files
+# Fortran modules
*.mod
*.smod
-# Compiled Static libraries
-*.lai
-*.la
-*.a
-*.lib
+# Dependency files
+*.d
-# Executables
-*.exe
-*.out
-*.app
+# ------------------------------------------------------------------------------
+# IDE and Editor files
+# ------------------------------------------------------------------------------
+
+# VSCode
+.vscode/
+*.code-workspace
+
+# JetBrains IDEs (CLion, etc.)
+.idea/
+
+# Vim
+*.swp
+*.swo
+
+# Emacs
+*~
+
+# ------------------------------------------------------------------------------
+# System files
+# ------------------------------------------------------------------------------
+
+.DS_Store
+Thumbs.db
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6c84a63..c6a1ed8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,21 +1,35 @@
cmake_minimum_required(VERSION 3.14)
-project(MyProject)
+project(ExoskeletonFirmware)
# Set C++ standard
set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
-# Add source files
-# add_library(MyLibrary src/my_library.cpp)
+# Enable testing
+enable_testing()
-# Include GoogleTest
+# Add googletest
add_subdirectory(googletest)
-# Enable testing
-enable_testing()
+# Motor API library
+add_library(motor_api
+ motors/motor_api.cpp
+)
+target_include_directories(motor_api PUBLIC motors)
+target_link_libraries(motor_api PRIVATE modbus)
+
+# Add test for motor API
+add_executable(motor_api_test
+ tests/motor_api_test.cpp
+)
+
+target_include_directories(motor_api_test PRIVATE motors)
+target_link_libraries(motor_api_test
+ motor_api
+ gtest
+ gtest_main
+)
-# Add unit tests
-add_executable(sampleTest tests/sample_test.cpp)
-target_link_libraries(sampleTest PRIVATE gtest gtest_main)
+add_test(NAME MotorAPITest COMMAND motor_api_test)
-# Register the test with CTest
-add_test(NAME sampleTest COMMAND sampleTest)
+# add more components later (sensors, control, etc.)
diff --git a/motors/README.md b/motors/README.md
index 5b004b3..933492a 100644
--- a/motors/README.md
+++ b/motors/README.md
@@ -1,2 +1,88 @@
-# Motor Control
-Submodule of McMaster exoskeleton interfacing and controlling suit motors
\ No newline at end of file
+# MotorAPI – High-Level Modbus RTU Motor Control
+
+This C++ library provides a high-level interface to control motors via Modbus RTU using [`libmodbus`](https://libmodbus.org/). It abstracts low-level register operations into simple function calls like `setSpeed()`, `setTorque()`, and `getActualPosition()`.
+
+## Features
+
+- Connects to motor over Modbus RTU via serial
+- Set motor torque, velocity, and position
+- Read actual motor torque, velocity, and position
+- Set operation modes (position, velocity, torque)
+- Includes safety checks and debug logging
+- GTest-compatible for unit testing #TODO
+
+## Dependencies
+
+- [libmodbus](https://libmodbus.org/)
+- C++17 or later
+- GoogleTest (optional for testing)
+
+## Usage
+
+### Initialization
+
+
+Motor motor("/dev/ttyUSB0", 1);
+if (!motor.initializeMotor()) {
+ std::cerr << "Failed to connect to motor.\n";
+}
+
+
+### Set Operation Mode
+
+
+motor.setOperationMode(PV_MODE); // PV_MODE, PT_MODE, or PP_MODE
+
+
+### Control Commands
+
+
+motor.setTargetVelocity(1000); // in INC/s
+motor.setTorque(500); // in ‰ (permille)
+motor.setPosition(100000); // in INC
+
+
+### Read Feedback
+
+
+int32_t pos = motor.getActualPosition();
+int32_t speed = motor.getActualVelocity();
+int16_t torque = motor.getActualTorque();
+
+
+### Cleanup
+
+
+motor.stopMotor();
+motor.disconnectMotor();
+
+
+## Running Tests (Optional)
+
+If hardware is connected and available:
+
+```bash
+mkdir build && cd build
+cmake ..
+make
+./motor_api_test
+```
+
+## Notes
+
+- `setTorque()` enforces bounds between -3000‰ and +3000‰.
+- `setMaxTorque()` only allows values up to 3000‰.
+- All `read*()` functions return zero if a read fails, and error messages are printed to `stderr`.
+- The motor must be placed in the correct operation mode (`PP_MODE`, `PV_MODE`, `PT_MODE`) before calling `setPosition()`, `setTargetVelocity()`, or `setTorque()`.
+- This API assumes motors follow the Modbus RTU protocol and register map of the EZmotion PRS/SRS R2 series.
+- Always verify that your power supply voltage and current limits match the motor specifications.
+- Communication defaults:
+ - Baud rate: `115200`
+ - Parity: Even (`'E'`)
+ - Data bits: 8
+ - Stop bits: 1
+
+## Reference
+
+This API is designed based on the **PRS SRS R2 Series User Guide** documentation.
+Refer to **Section 6: Register Table** in the official User Guide for detailed register definitions and explanations.
diff --git a/motors/motor_api.cpp b/motors/motor_api.cpp
new file mode 100644
index 0000000..be749fb
--- /dev/null
+++ b/motors/motor_api.cpp
@@ -0,0 +1,333 @@
+/*
+ * IMPORTANT: libmodbus is a C library, so we have to be careful and use c-style notation while using C++
+ * i.e. use C-style string literals instead of std::string
+ *
+ * Q: Why use libmodbus over a C++ library?
+ * A:
+ * - Most commonly used library for modbus RTU in C/C++.
+ * - Amazing Reference guide
+*/
+#include "motor_api.h"
+#include
+
+Motor::Motor(const std::string& dev, int id) : device(dev), slave_id(id), ctx(nullptr), connected(false) {}
+
+// Initialize and connect motor
+bool Motor::initializeMotor() {
+ ctx = modbus_new_rtu(device.c_str(), 115200, 'E', 8, 1);
+ if (!ctx) {
+ std::cerr << "Failed to create Modbus context for " << device << "\n";
+ return false;
+ }
+ if (modbus_set_slave(ctx, slave_id) == -1) {
+ std::cerr << "Invalid slave ID: " << slave_id << "\n";
+ modbus_free(ctx);
+ return false;
+ }
+ if (modbus_connect(ctx) == -1) {
+ std::cerr << "Failed to connect to " << device << ": " << modbus_strerror(errno) << "\n";
+ modbus_free(ctx);
+ return false;
+ }
+ connected = true;
+ std::cout << "Connected to motor on " << device << " (ID: " << slave_id << ")\n";
+ clearError();
+ writeRegister(REG_CONTROL_WORD, OPERATION_ENABLE);
+ setOperationMode(PT_MODE);
+ setMaxTorque(3000);
+ // Enable motor on startup
+
+ return true;
+}
+
+// Write a register
+bool Motor::writeRegister(uint16_t reg, uint16_t value) {
+ if (!connected) return false;
+ if (modbus_write_register(ctx, reg, value) == -1) {
+ std::cerr << "Modbus write error: " << modbus_strerror(errno) << "\n";
+ return false;
+ }
+ return true;
+}
+
+// Read a register
+bool Motor::readRegister(uint16_t reg, uint16_t &value) {
+ if (!connected) {
+ std::cerr << "[readRegister] Not connected to motor.\n";
+ return false;
+ }
+
+ if (modbus_read_registers(ctx, reg, 1, &value) == -1) {
+ std::cerr << "[readRegister] Modbus read error at 0x"
+ << std::hex << reg << ": "
+ << modbus_strerror(errno) << "\n";
+ return false;
+ }
+
+ return true;
+}
+
+void Motor::logErrorStatus() {
+ uint16_t error_status = 0;
+ if (!readRegister(REG_ERROR_STATUS, error_status)) {
+ std::cerr << "Failed to read Error Status register.\n";
+ return;
+ }
+
+ std::cout << "=== Motor Error Status (0x20BA) ===\n";
+ if (error_status == 0) {
+ std::cout << "No errors detected.\n";
+ return;
+ }
+
+ if (error_status & (1 << 0)) std::cout << "OCP: Over-current protection triggered.\n";
+ if (error_status & (1 << 1)) std::cout << "UVLO: Under-voltage lockout triggered.\n";
+ if (error_status & (1 << 2)) std::cout << "LOCK: Rotor lock detected.\n";
+ if (error_status & (1 << 3)) std::cout << "VIN_LIMIT: DC link voltage protection active.\n";
+ if (error_status & (1 << 4)) std::cout << "OVERLOAD: Overload protection triggered.\n";
+ if (error_status & (1 << 5)) std::cout << "POS_LIMIT: Position limit exceeded.\n";
+ if (error_status & (1 << 6)) std::cout << "SENSOR_ERR: Position sensor error.\n";
+ std::cout << "====================================\n";
+}
+
+
+// Write to a register of data type INT32
+bool Motor::writeRegister32(uint16_t start_addr, int32_t value) {
+ if (!connected) return false;
+ uint16_t regs[2] = {
+ static_cast(value & 0xFFFF),
+ static_cast((value >> 16) & 0xFFFF)
+ };
+ int rc = modbus_write_registers(ctx, start_addr, 2, regs);
+ if (rc != 2) {
+ std::cerr << "Failed to write 32-bit register: " << modbus_strerror(errno) << "\n";
+ return false;
+ }
+ return true;
+}
+bool Motor::clearError() {
+ // Step 1: Make sure bit 7 has a falling edge first
+ if (!writeRegister(REG_CONTROL_WORD, 0x0000)) return false;
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+ // Step 2: Write 0x0080 to trigger fault reset
+ if (!writeRegister(REG_CONTROL_WORD, 0x0080)) return false;
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+ // Step 3: Shutdown command (get to "Ready to switch on")
+ if (!writeRegister(REG_CONTROL_WORD, 0x0006)) return false;
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
+ return true;
+}
+
+// Read a register of data type INT32
+bool Motor::readRegister32(uint16_t start_addr, int32_t &out) {
+ if (!connected) {
+ std::cerr << "[readRegister32] Not connected to motor.\n";
+ return false;
+ }
+
+ uint16_t regs[2];
+ if (modbus_read_registers(ctx, start_addr, 2, regs) != 2) {
+ std::cerr << "[readRegister32] Modbus read error at 0x"
+ << std::hex << start_addr << ": "
+ << modbus_strerror(errno) << "\n";
+ return false;
+ }
+
+ out = (static_cast(regs[1]) << 16) | regs[0];
+ return true;
+}
+
+
+bool Motor::setOTPThreshold(int16_t temperature_C){
+ return writeRegister(REG_OTP_THRESHOLD, temperature_C);
+}
+
+
+
+bool Motor::setMotorAdrress(uint16_t adr){
+ return writeRegister(REG_MODBUS_ADDRESS, adr);
+}
+
+bool Motor::getMotorAddress(uint16_t &adr){
+ uint16_t address = 0;
+ if (!readRegister(REG_MODBUS_ADDRESS, address)) {
+ return false;
+ }
+ adr = address;
+ return true;
+}
+
+bool Motor::getMotorTemperature(int16_t &temperature_C){
+ int16_t temp = 0;
+ if (!readRegister(REG_TEMPERATURE, temp)) {
+ return false;
+ }
+ temperature_C = temp;
+ return true;
+}
+
+bool Motor::storeParameters(){
+ return writeRegister32(REG_RELATED_REGISTERS, STORE_PARAMETERS);
+}
+
+bool Motor::updateControlParameters(){
+ return writeRegister32(REG_RELATED_REGISTERS, UPDATE_CONTROL_PARAMETERS);
+}
+
+bool Motor::restoreDefaultParameters(){
+ return writeRegister32(REG_RELATED_REGISTERS, RESTORE_DEFAULT_PARAMETERS);
+}
+
+bool Motor::systemReset(){
+ return writeRegister32(REG_RELATED_REGISTERS, SYSTEM_RESET);
+}
+
+void Motor::printMotorAddress() {
+ uint16_t address = 0;
+ if (getMotorAddress(address)) {
+ std::cout << "Motor on " << device << " has address: " << address << std::endl;
+ } else {
+ std::cerr << "Failed to retrieve motor address for " << device << std::endl;
+ }
+}
+
+void Motor::printMotorTemperature() {
+ int16_t temperature = 0;
+ if (getMotorTemperature(temperature)) {
+ std::cout << "Motor on " << device << " has temperature: " << temperature << "°C" << std::endl;
+ } else {
+ std::cerr << "Failed to retrieve motor temperature for " << device << std::endl;
+ }
+}
+
+// Read raw current in ‰ (thousandths of rated torque)
+int16_t Motor::getActualCurrent() {
+ uint16_t raw_current;
+ if (!readRegister(REG_ACTUAL_CURRENT, raw_current)) {
+ return 0;
+ }
+ return static_cast(raw_current); // safe cast to signed
+}
+
+// TODO: DOUBLE CHECK BELOW CONSTANTS
+// Get actual current in milliamps
+double Motor::getActualCurrent_mA() {
+ // Ritvik: Current approach causes infinite reccursion
+ // I'm assuming this is a typo and getActualCurrent() was meant to be called
+ int16_t current_per_mille = getActualCurrent();
+
+ // Select rated torque based on motor model (update accordingly)
+ const double rated_torque = 0.64; // for 200W motor. in Nm
+ const double torque_constant = 0.056; // 56 mNm/A from user guide
+
+ // Convert current value from per-mille of rated torque to Nm
+ double actual_torque = (current_per_mille / 1000.0) * rated_torque;
+
+ double current_mA = actual_torque * 1000.0 / torque_constant;
+ return current_mA;
+}
+
+// Set motor position
+bool Motor::setPosition(int32_t position) {
+ return writeRegister32(REG_TARGET_POSITION, position);
+}
+
+// Get actual motor position
+int32_t Motor::getActualPosition() {
+ int32_t position;
+ if (!readRegister32(REG_ACTUAL_POSITION, position)) {
+ return 0;
+ }
+ return position;
+}
+
+// Get actual motor position in degrees
+double Motor::getDegrees(){
+ int32_t actual_position_in_INC = Motor::getActualPosition();
+ double degrees = (actual_position_in_INC * 360.0) / 65536.0;
+ return degrees;
+}
+
+// Set motor target velocity
+bool Motor::setTargetVelocity(int32_t velocity) {
+ return writeRegister32(REG_TARGET_VELOCITY, velocity);
+}
+
+// Get actual motor velocity
+int32_t Motor::getActualVelocity() {
+ int32_t velocity;
+ if (!readRegister32(REG_ACTUAL_VELOCITY, velocity)) {
+ return 0;
+ }
+ return velocity;
+}
+
+// Set motor target torque
+bool Motor::setTargetTorque(int16_t torque_permille) {
+ if (torque_permille < -3000 || torque_permille > 3000) {
+ std::cerr << "Torque value " << torque_permille << "‰ out of range [-3000, 3000]\n";
+ return false;
+ }
+ uint16_t raw = static_cast(*reinterpret_cast(&torque_permille));
+ return writeRegister(REG_TARGET_TORQUE, raw);
+}
+
+// Get actual motor torque
+int16_t Motor::getActualTorque() {
+ uint16_t torque = 0;
+ if (!readRegister(REG_ACTUAL_TORQUE, torque)) {
+ return 0;
+ }
+ return static_cast(torque);
+}
+
+// Set max motor torque
+bool Motor::setMaxTorque(uint16_t max_torque) {
+ if (max_torque > 3000) {
+ std::cerr << "Max torque exceeds 3000‰ limit.\n";
+ return false;
+ }
+ return writeRegister(REG_MAX_TORQUE, max_torque);
+}
+
+// Set motor operation mode
+bool Motor::setOperationMode(int16_t mode) {
+ return writeRegister(REG_OP_MODE, static_cast(mode));
+}
+
+// Get motor status
+uint16_t Motor::getStatus() {
+ uint16_t status = 0;
+ if (!readRegister(REG_CONTROL_WORD, status)) {
+ return 0;
+ }
+ return status;
+}
+
+
+
+// Stop motor
+bool Motor::stopMotor() {
+ return writeRegister(REG_CONTROL_WORD, OPERATION_DISABLE);
+}
+
+// Disconnect motor
+bool Motor::disconnectMotor() {
+ std::cout << "Disconnecting motor on " << device << " (ID: " << slave_id << ")\n";
+ if (connected) {
+ stopMotor();
+ modbus_close(ctx);
+ modbus_free(ctx);
+ ctx = nullptr;
+ connected = false;
+ return true;
+ }
+ return false;
+}
+
+// Destructor
+Motor::~Motor() {
+ disconnectMotor();
+}
\ No newline at end of file
diff --git a/motors/motor_api.h b/motors/motor_api.h
new file mode 100644
index 0000000..ffedbb6
--- /dev/null
+++ b/motors/motor_api.h
@@ -0,0 +1,120 @@
+#ifndef MOTOR_API_H
+#define MOTOR_API_H
+
+#include
+#include
+#include
+
+// ─────────────────────────────────────────────────────────────
+// Control Modes
+constexpr int16_t PP_MODE = 1; // Profile Position
+constexpr int16_t PV_MODE = 3; // Profile Velocity
+constexpr int16_t PT_MODE = 4; // Profile Torque
+
+// ─────────────────────────────────────────────────────────────
+// Control Words
+constexpr uint16_t OPERATION_ENABLE = 0x000F;
+constexpr uint16_t OPERATION_DISABLE = 0x0006;
+
+// ─────────────────────────────────────────────────────────────
+// Modbus Register Addresses
+constexpr uint16_t REG_OP_MODE = 0x6600;
+constexpr uint16_t REG_CONTROL_WORD = 0x6400;
+
+constexpr uint16_t REG_TARGET_TORQUE = 0x6710;
+constexpr uint16_t REG_TARGET_VELOCITY = 0x6FF0;
+constexpr uint16_t REG_TARGET_POSITION = 0x67A0;
+
+constexpr uint16_t REG_MAX_TORQUE = 0x6720;
+constexpr uint16_t REG_TRQ_SLOPE = 0x6870;
+
+constexpr uint16_t REG_ACTUAL_TORQUE = 0x6770;
+constexpr uint16_t REG_ACTUAL_VELOCITY = 0x66C0;
+constexpr uint16_t REG_ACTUAL_POSITION = 0x6640;
+constexpr uint16_t REG_ACTUAL_CURRENT = 0x6780;
+
+constexpr uint16_t REG_RATED_CURRENT = 0x2017;
+constexpr uint16_t REG_MODBUS_ADDRESS = 0x3050;
+
+constexpr uint32_t STORE_PARAMETERS = 0x65766173;
+constexpr uint32_t UPDATE_CONTROL_PARAMETERS = 0xAA5555AA;
+constexpr uint32_t SYSTEM_RESET = 0x626F6F74;
+constexpr uint32_t RESTORE_DEFAULT_PARAMETERS = 0x626F6F74;
+
+constexpr uint16_t REG_RELATED_REGISTERS= 0x20D0;
+
+constexpr uint16_t REG_ERROR_STATUS= 0x20BA;
+
+constexpr uint16_t REG_TEMPERATURE= 0x2400; //Indicates the PCB’s temperature
+constexpr uint16_t REG_OTP_THRESHOLD= 0x2410; //Sets the over-temperature protection (OTP) threshold
+
+
+// ─────────────────────────────────────────────────────────────
+// Motor Class
+class Motor {
+public:
+ // Constructor / Destructor
+ Motor(const std::string& device, int slave_id);
+ ~Motor();
+
+ // Initialization
+ bool initializeMotor();
+ bool disconnectMotor();
+
+ // Motor Control
+ bool setOperationMode(int16_t mode);
+ bool stopMotor();
+
+ bool setMaxTorque(uint16_t max_torque);
+ bool setTargetTorque(int16_t torque_permille);
+ int16_t getActualTorque();
+
+ bool setTargetVelocity(int32_t velocity);
+ int32_t getActualVelocity();
+
+ bool setPosition(int32_t position);
+ int32_t getActualPosition();
+ double getDegrees();
+
+ // bool setRatedCurrent(uint32_t current_mA);
+ uint32_t getRatedCurrent();
+ int16_t getActualCurrent();
+ double getActualCurrent_mA();
+
+ bool setMotorAdrress(uint16_t adr);
+ bool getMotorAddress(uint16_t &adr);
+ void printMotorAddress();
+
+ bool getMotorTemperature(int16_t &temperature_C);
+ void printMotorTemperature();
+ bool setOTPThreshold(int16_t temperature_C);
+
+
+ bool storeParameters();
+ bool updateControlParameters();
+ bool restoreDefaultParameters();
+ bool systemReset();
+
+ // Status
+ uint16_t getStatus();
+ void logErrorStatus();
+ bool isConnected() const { return connected; }
+ bool clearError();
+
+
+private:
+ std::string device;
+ int slave_id;
+ modbus_t* ctx;
+ bool connected;
+
+ // Register Access Helpers
+ bool writeRegister(uint16_t reg, uint16_t value);
+ bool readRegister(uint16_t reg, uint16_t &value);
+
+ // For 32-bit register access
+ bool writeRegister32(uint16_t start_addr, int32_t value);
+ bool readRegister32(uint16_t start_addr, int32_t &out);
+};
+
+#endif // MOTOR_API_H
\ No newline at end of file
diff --git a/sensors/imu_filter.cpp b/sensors/imu_filter.cpp
new file mode 100644
index 0000000..c4f7684
--- /dev/null
+++ b/sensors/imu_filter.cpp
@@ -0,0 +1,153 @@
+#include "one_d_kalman_filter.h"
+#include "low_pass_filter.h"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+using json = nlohmann::json;
+
+// Structure to hold IMU data
+struct IMUData {
+ std::string timestamp;
+ std::string location;
+ Eigen::VectorXd measurement; // heading, pitch, roll
+};
+
+// Read IMU data from JSON file
+std::vector read_imu_data(const std::string &filename) {
+ std::vector imu_data;
+ std::ifstream file(filename);
+ std::string line;
+
+ if (!file.is_open()) {
+ std::cerr << "Failed to open file: " << filename << std::endl;
+ return imu_data;
+ }
+
+ while (std::getline(file, line)) {
+ try {
+ json entry = json::parse(line);
+ std::string timestamp = entry["timestamp"];
+
+ for (const auto &sensor : entry["sensors"]) {
+ IMUData data;
+ data.timestamp = timestamp;
+ data.location = sensor["location"];
+
+ data.measurement = Eigen::VectorXd(3);
+ data.measurement << sensor["euler"]["heading"],
+ sensor["euler"]["pitch"],
+ sensor["euler"]["roll"];
+
+ imu_data.push_back(data);
+ }
+ } catch (const json::exception &e) {
+ std::cerr << "JSON parsing error: " << e.what() << std::endl;
+ continue;
+ }
+ }
+
+ return imu_data;
+}
+
+// Apply 1D Kalman filter per Euler component and sensor
+void apply_kalman_filter(std::vector &imu_data) {
+ std::unordered_map> filters;
+
+ std::ofstream diff_file("filtered_differences.txt");
+ if (!diff_file.is_open()) {
+ std::cerr << "Failed to open filtered_differences.txt" << std::endl;
+ return;
+ }
+
+ for (auto &data : imu_data) {
+ auto &filter_array = filters[data.location];
+
+ // If not initialized yet, construct filters with initial values
+ if (filter_array[0].update(0) == 0.0 && filter_array[1].update(0) == 0.0 && filter_array[2].update(0) == 0.0) {
+ filter_array[0] = OneDKalmanFilter(0.01, 1.0, data.measurement(0));
+ filter_array[1] = OneDKalmanFilter(0.01, 1.0, data.measurement(1));
+ filter_array[2] = OneDKalmanFilter(0.01, 1.0, data.measurement(2));
+ }
+
+ Eigen::VectorXd prev = data.measurement;
+
+ data.measurement(0) = filter_array[0].update(data.measurement(0));
+ data.measurement(1) = filter_array[1].update(data.measurement(1));
+ data.measurement(2) = filter_array[2].update(data.measurement(2));
+
+ diff_file << "Sensor: " << data.location << "\n";
+ diff_file << "Timestamp: " << data.timestamp << "\n";
+ diff_file << "Before filtering: H=" << prev(0)
+ << ", P=" << prev(1)
+ << ", R=" << prev(2) << "\n";
+ diff_file << "After filtering: H=" << data.measurement(0)
+ << ", P=" << data.measurement(1)
+ << ", R=" << data.measurement(2) << "\n\n";
+ }
+
+ diff_file.close();
+ std::cout << "Filtered differences saved to filtered_differences.txt" << std::endl;
+}
+
+// Save filtered IMU data while keeping original format
+void save_filtered_data(const std::vector &imu_data, const std::string &filename, const std::string &original_filename) {
+ std::ofstream file(filename);
+ if (!file.is_open()) {
+ std::cerr << "Failed to open file: " << filename << std::endl;
+ return;
+ }
+
+ std::ifstream original_file(original_filename);
+ if (!original_file.is_open()) {
+ std::cerr << "Failed to open original file: " << original_filename << std::endl;
+ return;
+ }
+
+ std::string line;
+ size_t index = 0;
+
+ while (std::getline(original_file, line)) {
+ json entry = json::parse(line);
+ json filtered_entry;
+ filtered_entry["timestamp"] = entry["timestamp"];
+ filtered_entry["sensors"] = json::array();
+
+ for (size_t i = 0; i < entry["sensors"].size(); ++i) {
+ json sensor = entry["sensors"][i];
+
+ if (index < imu_data.size() && imu_data[index].location == sensor["location"]) {
+ sensor["euler"]["heading"] = imu_data[index].measurement(0);
+ sensor["euler"]["pitch"] = imu_data[index].measurement(1);
+ sensor["euler"]["roll"] = imu_data[index].measurement(2);
+ index++;
+ }
+
+ filtered_entry["sensors"].push_back(sensor);
+ }
+
+ file << filtered_entry.dump() << std::endl;
+ }
+
+ std::cout << "Filtered data saved to " << filename << std::endl;
+}
+
+int main() {
+ std::string input_file = "C:/Users/Luis/Documents/Exoskeleton/Sensor-logging-branch/Exoskeleton-Embedded/sensors/treadmill_imu_data/imu_data_treadmill_5min_1.9mph.json";
+ std::string output_file = "C:/Users/Luis/Documents/Exoskeleton/Sensor-logging-branch/Exoskeleton-Embedded/sensors/treadmill_imu_data/filtered_imu_data.json";
+
+ std::vector imu_data = read_imu_data(input_file);
+ if (imu_data.empty()) {
+ std::cerr << "No IMU data found." << std::endl;
+ return -1;
+ }
+
+ apply_kalman_filter(imu_data);
+ save_filtered_data(imu_data, output_file, input_file);
+
+ return 0;
+}
diff --git a/sensors/imu_filter_lpf.cpp b/sensors/imu_filter_lpf.cpp
new file mode 100644
index 0000000..9b698c8
--- /dev/null
+++ b/sensors/imu_filter_lpf.cpp
@@ -0,0 +1,147 @@
+#include "low_pass_filter.h"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+using json = nlohmann::json;
+
+// Structure to hold IMU data
+struct IMUData {
+ std::string timestamp;
+ std::string location;
+ Eigen::VectorXd measurement; // heading, pitch, roll
+};
+
+// Read IMU data from JSON file
+std::vector read_imu_data(const std::string &filename) {
+ std::vector imu_data;
+ std::ifstream file(filename);
+ std::string line;
+
+ if (!file.is_open()) {
+ std::cerr << "Failed to open file: " << filename << std::endl;
+ return imu_data;
+ }
+
+ while (std::getline(file, line)) {
+ try {
+ json entry = json::parse(line);
+ std::string timestamp = entry["timestamp"];
+
+ for (const auto &sensor : entry["sensors"]) {
+ IMUData data;
+ data.timestamp = timestamp;
+ data.location = sensor["location"];
+
+ data.measurement = Eigen::VectorXd(3);
+ data.measurement << sensor["euler"]["heading"],
+ sensor["euler"]["pitch"],
+ sensor["euler"]["roll"];
+
+ imu_data.push_back(data);
+ }
+ } catch (const json::exception &e) {
+ std::cerr << "JSON parsing error: " << e.what() << std::endl;
+ continue;
+ }
+ }
+
+ return imu_data;
+}
+
+// Apply low-pass filter per location and Euler component
+void apply_low_pass_filter(std::vector &imu_data, double alpha = 0.1) {
+ std::unordered_map> filters;
+
+ std::ofstream log_file("filtered_differences_lpf.txt");
+ if (!log_file.is_open()) {
+ std::cerr << "Failed to open filtered_differences_lpf.txt" << std::endl;
+ return;
+ }
+
+ for (auto &data : imu_data) {
+ auto &filter_array = filters[data.location];
+
+ for (int i = 0; i < 3; ++i) {
+ if (!filter_array[i].is_initialized()) {
+ filter_array[i] = LowPassFilter(alpha);
+ filter_array[i].initialize(data.measurement(i));
+ }
+
+ double before = data.measurement(i);
+ double after = filter_array[i].update(data.measurement(i));
+ data.measurement(i) = after;
+
+ log_file << "Sensor: " << data.location << ", Timestamp: " << data.timestamp << "\n";
+ log_file << "Component " << i << " - Before: " << before << ", After: " << after << "\n";
+ }
+
+ log_file << "\n";
+ }
+
+ log_file.close();
+ std::cout << "LPF differences saved to filtered_differences_lpf.txt\n";
+}
+
+// Save filtered IMU data while keeping original format
+void save_filtered_lpf(const std::vector &imu_data, const std::string &filename, const std::string &original_filename) {
+ std::ofstream file(filename);
+ if (!file.is_open()) {
+ std::cerr << "Failed to open file: " << filename << std::endl;
+ return;
+ }
+
+ std::ifstream original_file(original_filename);
+ if (!original_file.is_open()) {
+ std::cerr << "Failed to open original file: " << original_filename << std::endl;
+ return;
+ }
+
+ std::string line;
+ size_t index = 0;
+
+ while (std::getline(original_file, line)) {
+ json entry = json::parse(line);
+ json filtered_entry;
+ filtered_entry["timestamp"] = entry["timestamp"];
+ filtered_entry["sensors"] = json::array();
+
+ for (size_t i = 0; i < entry["sensors"].size(); ++i) {
+ json sensor = entry["sensors"][i];
+
+ if (index < imu_data.size() && imu_data[index].location == sensor["location"]) {
+ sensor["euler"]["heading"] = imu_data[index].measurement(0);
+ sensor["euler"]["pitch"] = imu_data[index].measurement(1);
+ sensor["euler"]["roll"] = imu_data[index].measurement(2);
+ index++;
+ }
+
+ filtered_entry["sensors"].push_back(sensor);
+ }
+
+ file << filtered_entry.dump() << std::endl;
+ }
+
+ std::cout << "LPF-filtered data saved to " << filename << std::endl;
+}
+
+// Example usage in main
+int main() {
+ std::string input_file = "sensors/treadmill_imu_data/imu_data_treadmill_5min_1.9mph.json";
+ std::string output_file_lpf = "sensors/treadmill_imu_data/filtered_imu_data_lpf.json";
+
+ std::vector imu_data = read_imu_data(input_file);
+ if (imu_data.empty()) {
+ std::cerr << "No IMU data found." << std::endl;
+ return -1;
+ }
+
+ apply_low_pass_filter(imu_data, 0.1); // Use smoothing factor alpha = 0.1
+ save_filtered_lpf(imu_data, output_file_lpf, input_file);
+
+ return 0;
+}
diff --git a/sensors/low_pass_filter.h b/sensors/low_pass_filter.h
new file mode 100644
index 0000000..3d96a4b
--- /dev/null
+++ b/sensors/low_pass_filter.h
@@ -0,0 +1,35 @@
+#ifndef LOW_PASS_FILTER_H
+#define LOW_PASS_FILTER_H
+
+class LowPassFilter {
+public:
+ // Default constructor required for std::array
+ LowPassFilter() : alpha(0.5), initialized(false), filtered_value(0.0) {}
+
+ // Constructor with custom alpha
+ LowPassFilter(double alpha) : alpha(alpha), initialized(false), filtered_value(0.0) {}
+
+ void initialize(double value) {
+ filtered_value = value;
+ initialized = true;
+ }
+
+ double update(double measurement) {
+ if (!initialized) {
+ initialize(measurement);
+ }
+ filtered_value = alpha * measurement + (1 - alpha) * filtered_value;
+ return filtered_value;
+ }
+
+ bool is_initialized() const {
+ return initialized;
+ }
+
+private:
+ double alpha;
+ double filtered_value;
+ bool initialized;
+};
+
+#endif // LOW_PASS_FILTER_H
diff --git a/sensors/normalize_sensor_data.py b/sensors/normalize_sensor_data.py
new file mode 100644
index 0000000..ecb21ae
--- /dev/null
+++ b/sensors/normalize_sensor_data.py
@@ -0,0 +1,85 @@
+"""
+To get rid of sensor drift that occurs when collecting sensor value. Normalize readings on y axis.
+"""
+
+import sys
+import json
+
+def normalize(data,section_size=100):
+ if len(data) < section_size:
+ section_size = len(data)
+
+ total = sum(data[:section_size])
+ translation = 0 - total/section_size
+ new_data = []
+
+ for i in range(section_size):
+ new_data.append(data[i] + translation)
+
+ for i in range(section_size, len(data)):
+ total = total + data[i] - data[i-section_size]
+ translation = -total/section_size
+ new_data.append(data[i] + translation)
+
+ return new_data
+
+def fix_flip(data,diff_max = 200,section_size=100):
+ new_data = [data[0]]
+ for i in range(1,len(data)):
+ diff = data[i]-sum(new_data[-section_size:])/min(section_size,len(new_data))
+ if diff > diff_max:
+ new_data.append(data[i] - 360)
+ elif diff < -diff_max:
+ new_data.append(data[i] + 360)
+ else:
+ new_data.append(data[i])
+
+ return new_data
+
+def strip_data(data):
+ while data[0] == 0:
+ data.pop(0)
+
+ while data[-1] == 0:
+ data.pop()
+
+def apply_preprocess(sensor_data,f):
+ sensors = [x["location"] for x in sensor_data[0]["sensors"]]
+ angle_names = [x for x in sensor_data[0]["sensors"][0]["euler"]]
+
+ for si,sensor in enumerate(sensors):
+ for angle in angle_names:
+ data = [x["sensors"][si]["euler"][angle] for x in sensor_data]
+ data = f(data)
+ for i in range(len(sensor_data)):
+ sensor_data[i]["sensors"][si]["euler"][angle] = data[i]
+
+def process_file(file_path,output_path):
+ sensor_data = []
+
+ with open(file_path,"r") as file:
+ for line in file:
+ sensor_data.append(json.loads(line))
+
+ apply_preprocess(sensor_data,fix_flip)
+ apply_preprocess(sensor_data,normalize)
+
+ print(f"\n\nsensor data: {sensor_data[0]}")
+ print(f"\n\nsensor data type: {type(sensor_data[0])}\n\n")
+
+ with open(output_path,"w") as file:
+ for i in range(len(sensor_data)):
+ file.write(json.dumps(sensor_data[i]) + "\n")
+
+
+def main():
+ if len(sys.argv) < 3:
+ print("Usage: python script.py ",file=sys.stderr)
+ sys.exit(1)
+
+ file_path = sys.argv[1]
+ output_path = sys.argv[2]
+ process_file(file_path,output_path)
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/sensors/normalize_sensor_data_c++.cpp b/sensors/normalize_sensor_data_c++.cpp
new file mode 100644
index 0000000..87449fb
--- /dev/null
+++ b/sensors/normalize_sensor_data_c++.cpp
@@ -0,0 +1,154 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "nlohmann/json.hpp"
+using json = nlohmann::json;
+
+std::vector normalize(const std::vector& data, int section_size = 100) {
+ int n = data.size();
+ int sec = section_size;
+ if (n < sec) {
+ sec = n;
+ }
+ double total = 0.0;
+ for (int i = 0; i < sec; i++) {
+ total += data[i];
+ }
+ double translation = -total / sec;
+ std::vector new_data;
+ new_data.reserve(n);
+
+ for (int i = 0; i < sec; i++) {
+ new_data.push_back(data[i] + translation);
+ }
+ for (int i = sec; i < n; i++) {
+ total = total + data[i] - data[i - sec];
+ translation = -total / sec;
+ new_data.push_back(data[i] + translation);
+ }
+ return new_data;
+}
+
+std::vector fix_flip(const std::vector& data, double diff_max = 200, int section_size = 100) {
+ std::vector new_data;
+ if (data.empty()) {
+ return new_data;
+ }
+ new_data.push_back(data[0]);
+ for (size_t i = 1; i < data.size(); i++) {
+ int window_size = std::min(section_size, static_cast(new_data.size()));
+ double sum_window = 0;
+ for (int j = new_data.size() - window_size; j < new_data.size(); j++) {
+ sum_window += new_data[j];
+ }
+ double average = sum_window / window_size;
+ double diff = data[i] - average;
+ if (diff > diff_max) {
+ new_data.push_back(data[i] - 360);
+ } else if (diff < -diff_max) {
+ new_data.push_back(data[i] + 360);
+ } else {
+ new_data.push_back(data[i]);
+ }
+ }
+ return new_data;
+}
+
+void strip_data(std::vector& data) {
+ while (!data.empty() && data.front() == 0) {
+ data.erase(data.begin());
+ }
+ while (!data.empty() && data.back() == 0) {
+ data.pop_back();
+ }
+}
+
+void apply_preprocess(std::vector& sensor_data,
+ const std::function(const std::vector&)>& f) {
+ if (sensor_data.empty()) {
+ return;
+ }
+ size_t numSensors = sensor_data[0]["sensors"].size();
+
+ std::vector angle_names;
+ for (auto& item : sensor_data[0]["sensors"][0]["euler"].items()) {
+ angle_names.push_back(item.key());
+ }
+
+ // For each sensor and each angle, extract the time-series, process it, then update.
+ for (size_t si = 0; si < numSensors; si++) {
+ for (const auto& angle : angle_names) {
+ std::vector data;
+ data.reserve(sensor_data.size());
+ for (auto& snapshot : sensor_data) {
+ data.push_back(snapshot["sensors"][si]["euler"][angle].get());
+ }
+ std::vector new_data = f(data);
+
+ for (size_t i = 0; i < sensor_data.size(); i++) {
+ sensor_data[i]["sensors"][si]["euler"][angle] = new_data[i];
+ }
+ }
+ }
+}
+
+void process_file(const std::string& file_path, const std::string& output_path) {
+ std::vector sensor_data;
+ std::ifstream infile(file_path);
+ if (!infile) {
+ std::cerr << "Error opening input file: " << file_path << std::endl;
+ std::exit(EXIT_FAILURE);
+ }
+
+ std::string line;
+ while (std::getline(infile, line)) {
+ if (!line.empty()) {
+ try {
+ sensor_data.push_back(json::parse(line));
+ } catch (const std::exception& e) {
+ std::cerr << "Error parsing JSON: " << e.what() << std::endl;
+ }
+ }
+ }
+ infile.close();
+
+ apply_preprocess(sensor_data, [](const std::vector& data) {
+ return fix_flip(data);
+ });
+ apply_preprocess(sensor_data, [](const std::vector& data) {
+ return normalize(data);
+ });
+
+
+ // Write processed sensor data back to the output file (one JSON per line).
+ std::ofstream outfile(output_path);
+ if (!outfile) {
+ std::cerr << "Error opening output file: " << output_path << std::endl;
+ std::exit(EXIT_FAILURE);
+ }
+ for (const auto& snapshot : sensor_data) {
+ outfile << snapshot.dump() << "\n";
+ }
+ outfile.close();
+}
+
+int main(int argc, char* argv[]) {
+ if (argc < 3) {
+ std::cerr << "Usage: " << argv[0] << "