From 3be961eecf5973a1dbaac99048632b303786e647 Mon Sep 17 00:00:00 2001 From: 0x23 Date: Sat, 18 Oct 2025 12:43:13 +0200 Subject: [PATCH] - added raw logging - better calibration output - added calibration plotter --- firmware/MotionControllerRP/platformio.ini | 4 +- .../servo_control/actuator_calibration.cpp | 2 +- .../src/utilities/logging.cpp | 7 ++++ .../src/utilities/logging.h | 2 + firmware/MotionControllerRP/src/version.h | 2 +- software/PythonAPI/calibration_plotter.py | 40 +++++++++++++++++++ 6 files changed, 53 insertions(+), 4 deletions(-) create mode 100644 software/PythonAPI/calibration_plotter.py diff --git a/firmware/MotionControllerRP/platformio.ini b/firmware/MotionControllerRP/platformio.ini index 19f0f46..7c90df7 100644 --- a/firmware/MotionControllerRP/platformio.ini +++ b/firmware/MotionControllerRP/platformio.ini @@ -23,8 +23,8 @@ [env:pico] platform = https://github.com/maxgerhardt/platform-raspberrypi.git -board = rpipico2 # RP2530 -# board = rpipico # RP2040 +board = rpipico2 # use this for RP2530 +# board = rpipico # use this for RP2040 board_build.filesystem_size = 0.5m framework = arduino diff --git a/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp index ab8bed2..0f1d7a6 100644 --- a/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp +++ b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp @@ -49,7 +49,7 @@ bool measure_calibration_data( // TODO: read motor_pos from precise reference encoder if(print_measurements) - LOG_INFO("%f, %15.10f, %15.10f", encoder_angle_raw, field_angle, motor_pos); + LOG_RAW("%15.10f, %15.10f, %f", motor_pos, field_angle, encoder_angle_raw); encoder_angle_and_motor_pos.push_back({encoder_angle_raw, motor_pos}); motor_pos_and_field_angle.push_back({motor_pos, field_angle}); diff --git a/firmware/MotionControllerRP/src/utilities/logging.cpp b/firmware/MotionControllerRP/src/utilities/logging.cpp index 47c42fa..8cbfef6 100644 --- a/firmware/MotionControllerRP/src/utilities/logging.cpp +++ b/firmware/MotionControllerRP/src/utilities/logging.cpp @@ -60,6 +60,13 @@ void Logger::error(const char* fmt, ...) { } } +void Logger::raw(const char* fmt, ...) { + va_list args; + va_start(args, fmt); + log(ELogLevel::NONE, fmt, args); + va_end(args); +} + void Logger::log(ELogLevel level, const char* fmt, va_list args) { char buf[128]; // Adjust size as needed vsnprintf(buf, sizeof(buf), fmt, args); diff --git a/firmware/MotionControllerRP/src/utilities/logging.h b/firmware/MotionControllerRP/src/utilities/logging.h index 3d46de2..b4f8473 100644 --- a/firmware/MotionControllerRP/src/utilities/logging.h +++ b/firmware/MotionControllerRP/src/utilities/logging.h @@ -15,6 +15,7 @@ #define LOG_INFO(...) Logger::instance().info(__VA_ARGS__) #define LOG_WARNING(...) Logger::instance().warn(__VA_ARGS__) #define LOG_ERROR(...) Logger::instance().error(__VA_ARGS__) +#define LOG_RAW(...) Logger::instance().raw(__VA_ARGS__) //*** ENUM ****************************************************************************** @@ -39,6 +40,7 @@ class Logger { void info(const char* fmt, ...); void warn(const char* fmt, ...); void error(const char* fmt, ...); + void raw(const char* fmt, ...); private: Logger() = default; diff --git a/firmware/MotionControllerRP/src/version.h b/firmware/MotionControllerRP/src/version.h index b5f5346..e6baba0 100644 --- a/firmware/MotionControllerRP/src/version.h +++ b/firmware/MotionControllerRP/src/version.h @@ -1 +1 @@ -static const char* FIRMWARE_VERSION = "v1.0.1"; +static const char* FIRMWARE_VERSION = "v1.0.2"; diff --git a/software/PythonAPI/calibration_plotter.py b/software/PythonAPI/calibration_plotter.py new file mode 100644 index 0000000..c0e3f26 --- /dev/null +++ b/software/PythonAPI/calibration_plotter.py @@ -0,0 +1,40 @@ +from open_micro_stage_api import OpenMicroStageInterface +import matplotlib.pyplot as plt +plt.rcParams['figure.dpi'] = 200 + +def plot_calibration_data(ax_encoder_counts, ax_field_angel, label, data): + # Plot on the provided Axes object + if ax_encoder_counts is not None: + ax_encoder_counts.plot(data[0], data[2], label=label) + ax_encoder_counts.set_xlabel('Motor Angle [rad]') + ax_encoder_counts.set_ylabel('Encoder Counts Raw') + ax_encoder_counts.set_title('Encoder Count Plot') + ax_encoder_counts.legend() + ax_encoder_counts.grid(True) + + # Plot on the provided Axes object + if ax_field_angel is not None: + ax_field_angel.plot(data[0], data[1], label=label) + ax_field_angel.set_xlabel('Motor Angle [rad]') + ax_field_angel.set_ylabel('Motor Field Angle [rad]') + ax_field_angel.set_title('Field Angle Plot') + ax_field_angel.legend() + ax_field_angel.grid(True) + +def main(): + # create interface and connect + oms = OpenMicroStageInterface(show_communication=True, show_log_messages=True) + oms.connect('/dev/ttyACM0') + + # Create subplots + fig, ax = plt.subplots(1, 1, figsize=(10, 7), sharex='all') + + for i in range(3): + res, data = oms.calibrate_joint(i, save_result=False) + plot_calibration_data(ax, None, f'Actuator {i}', data) + + # Adjust layout and show + plt.tight_layout() + plt.show() + +main() \ No newline at end of file