From 71e59e20bbd145850c631ccd4557ccba83260d43 Mon Sep 17 00:00:00 2001 From: 0x23 Date: Mon, 13 Oct 2025 23:45:32 +0200 Subject: [PATCH] added measurement print option to calibration --- firmware/MotionControllerRP/src/hw_config.h | 2 +- firmware/MotionControllerRP/src/main.cpp | 1 - firmware/MotionControllerRP/src/robot.cpp | 13 ++++++++----- firmware/MotionControllerRP/src/robot.h | 4 ++-- .../src/servo_control/actuator_calibration.cpp | 6 +++++- .../src/servo_control/actuator_calibration.h | 3 ++- 6 files changed, 18 insertions(+), 11 deletions(-) diff --git a/firmware/MotionControllerRP/src/hw_config.h b/firmware/MotionControllerRP/src/hw_config.h index d296c82..e380e33 100644 --- a/firmware/MotionControllerRP/src/hw_config.h +++ b/firmware/MotionControllerRP/src/hw_config.h @@ -13,7 +13,7 @@ #define HOMING_FINISH_POS 0.5f // in rad -#define SINGLE_AXIS_BOARD +// #define SINGLE_AXIS_BOARD #ifndef SINGLE_AXIS_BOARD // Pins for 3Axis Board #define PIN_BUILTIN_LED 23 diff --git a/firmware/MotionControllerRP/src/main.cpp b/firmware/MotionControllerRP/src/main.cpp index ef10bad..e7f40fd 100644 --- a/firmware/MotionControllerRP/src/main.cpp +++ b/firmware/MotionControllerRP/src/main.cpp @@ -68,7 +68,6 @@ void overclock() { } void set_led_color(uint8_t r, uint8_t g, uint8_t b) { - return; strip.neoPixelSetValue(0, r, g, b, false); delayMicroseconds(2000); strip.neoPixelShow(); diff --git a/firmware/MotionControllerRP/src/robot.cpp b/firmware/MotionControllerRP/src/robot.cpp index 2bc6075..8bb991f 100644 --- a/firmware/MotionControllerRP/src/robot.cpp +++ b/firmware/MotionControllerRP/src/robot.cpp @@ -58,7 +58,7 @@ void RobotJoint::init(int joint_idx) { servo_controller->set_motor_enabled(false, false); } -bool RobotJoint::calibrate() { +bool RobotJoint::calibrate(bool print_measurements) { LOG_INFO("Joint-%i: calibrating joint...", joint_idx); HomingController homing_controller; @@ -77,7 +77,8 @@ bool RobotJoint::calibrate() { *servo_controller, CALIBRATION_RANGE*DEG_TO_RAD, CALIBRATION_FIELD_VELOCITY, - 256); + 256, + print_measurements); if(!ok) { LOG_ERROR("Joint-%i: calibrating failed", joint_idx); return false; @@ -468,7 +469,7 @@ bool Robot::home(uint8_t joint_mask) { return homing_successful; } -bool Robot::calibrate_joint(int joint_idx, bool store_calibration) { +bool Robot::calibrate_joint(int joint_idx, bool store_calibration, bool print_measurements) { if(joint_idx<0 || joint_idx >= NUM_JOINTS) return false; @@ -478,7 +479,7 @@ bool Robot::calibrate_joint(int joint_idx, bool store_calibration) { enable_servo_control(false); spin_lock_unsafe_blocking(joints_spin_lock); - bool calibration_ok = joint->calibrate(); + bool calibration_ok = joint->calibrate(print_measurements); if(!calibration_ok) { spin_unlock_unsafe(joints_spin_lock); return false; @@ -634,6 +635,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply) reply += "ok\n"; } + // print lookup table if(cmd.get_command() == "M59") { int idx = (int)cmd.get_value('J', 0); joints[idx]->servo_controller->get_enc_to_pos_lut().print_to_log(); @@ -776,7 +778,8 @@ void Robot::process_home_command(const GCodeCommand& cmd, std::string& reply) { void Robot::process_calibrate_joint_command(const GCodeCommand& cmd, std::string& reply) { int idx = cmd.get_value('J', 0); bool store_calibration = cmd.has_word('S'); + bool print_measurements = cmd.has_word('P'); - bool ok = calibrate_joint(idx, store_calibration); + bool ok = calibrate_joint(idx, store_calibration, print_measurements); reply = ok ? "ok\n" : "error\n"; } \ No newline at end of file diff --git a/firmware/MotionControllerRP/src/robot.h b/firmware/MotionControllerRP/src/robot.h index fd2abfe..a2b3cef 100644 --- a/firmware/MotionControllerRP/src/robot.h +++ b/firmware/MotionControllerRP/src/robot.h @@ -62,7 +62,7 @@ class RobotJoint { ~RobotJoint(); void init(int joint_idx); - bool calibrate(); + bool calibrate(bool print_measurements); void update(float dt, float one_over_dt); void update_target(float p, float v); bool load_calibration(); @@ -94,7 +94,7 @@ class Robot : public ICommandProcessor { void init(); void calibrate(); bool home(uint8_t joint_mask=255); - bool calibrate_joint(int joint_idx, bool store_calibration); + bool calibrate_joint(int joint_idx, bool store_calibration, bool print_measurements); void enable_servo_control(bool enable); // enables joint servo controll if homed and calibrated void update_command_parser(); // called from main loop diff --git a/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp index d681f4b..ab8bed2 100644 --- a/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp +++ b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp @@ -22,7 +22,8 @@ bool measure_calibration_data( ServoController& servo_controller, float calibration_range, float field_velocity, - size_t table_size) + size_t table_size, + bool print_measurements) { LOG_INFO("Measuring motor to encoder angle lookup table..."); int sample_count = table_size*4; @@ -47,6 +48,9 @@ bool measure_calibration_data( float motor_pos = (field_angle-start_field_angle)/pole_pair_count; // 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); + 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/servo_control/actuator_calibration.h b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.h index 912be5c..0dbc344 100644 --- a/firmware/MotionControllerRP/src/servo_control/actuator_calibration.h +++ b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.h @@ -18,5 +18,6 @@ bool measure_calibration_data( ServoController& servo_controller, float field_angle_range, float field_velocity, - size_t size); + size_t size, + bool print_measurements);