From 207c0b890459ca7c7b9d317e92728464da113592 Mon Sep 17 00:00:00 2001 From: Capo01 <503426+Capo01@users.noreply.github.com> Date: Fri, 16 Feb 2018 20:49:28 +1100 Subject: [PATCH 1/7] Update low_level.c --- Firmware/MotorControl/low_level.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index c49b95ea..34082679 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -70,8 +70,8 @@ Motor_t motors[] = { .vel_integrator_current = 0.0f, // [A] .vel_limit = 20000.0f, // [counts/s] .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .resistance_calib_max_voltage = 1.0f, // [V] + .calibration_current = 10.0f, // [A] If > 10A then resistance_calib_max_voltage may need to be increased + .resistance_calib_max_voltage = 1.0f, // [V] .phase_inductance = 0.0f, // to be set by measure_phase_inductance .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, From dbb78a160de59554ff627dd4c96ec33fa6f090df Mon Sep 17 00:00:00 2001 From: Marc Freudenberg Date: Fri, 16 Feb 2018 22:54:55 +0100 Subject: [PATCH 2/7] Update protocol.md --- Firmware/protocol.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/protocol.md b/Firmware/protocol.md index ce22164e..1a3bae4f 100644 --- a/Firmware/protocol.md +++ b/Firmware/protocol.md @@ -26,8 +26,8 @@ by default while UART runs the stream based variant. ## Packet format ## We will call the ODrive "server" and the PC "client". A request is a message -from the PC to the ODrive and a response is a message from the PC to the -ODrive. +from the PC to the ODrive and a response is a message from the ODrive to the +PC. Each request-response transaction corresponds to a single endpoint operation. From d388ea54c1878c503599064c005040a905a2e2e3 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 16 Feb 2018 14:49:10 -0800 Subject: [PATCH 3/7] Update low_level.c --- Firmware/MotorControl/low_level.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 34082679..96826b0d 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -70,8 +70,8 @@ Motor_t motors[] = { .vel_integrator_current = 0.0f, // [A] .vel_limit = 20000.0f, // [counts/s] .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] If > 10A then resistance_calib_max_voltage may need to be increased - .resistance_calib_max_voltage = 1.0f, // [V] + .calibration_current = 10.0f, // [A] + .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. .phase_inductance = 0.0f, // to be set by measure_phase_inductance .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, @@ -176,7 +176,7 @@ Motor_t motors[] = { .vel_limit = 20000.0f, // [counts/s] .current_setpoint = 0.0f, // [A] .calibration_current = 10.0f, // [A] - .resistance_calib_max_voltage = 1.0f, // [V] + .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. .phase_inductance = 0.0f, // to be set by measure_phase_inductance .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, From 06ab4d7bae930c46dd98c75c55ca6a136f5a61f1 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Mon, 19 Feb 2018 19:42:47 -0500 Subject: [PATCH 4/7] Add Encoder CPR range checking --- Firmware/MotorControl/low_level.c | 17 ++++++++++++++--- Firmware/MotorControl/low_level.h | 2 ++ 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 96826b0d..d0a7ff5f 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -129,6 +129,7 @@ Motor_t motors[] = { .encoder_offset = 0, .encoder_state = 0, .motor_dir = 1, // 1 or -1 + .encoder_calib_range = 0.02, .phase = 0.0f, // [rad] .pll_pos = 0.0f, // [rad] .pll_vel = 0.0f, // [rad/s] @@ -231,6 +232,7 @@ Motor_t motors[] = { .encoder_offset = 0, .encoder_state = 0, .motor_dir = 1, // 1 or -1 + .encoder_calib_range = 0.02, .phase = 0.0f, // [rad] .pll_pos = 0.0f, // [rad] .pll_vel = 0.0f, // [rad/s] @@ -765,9 +767,6 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { static const float scan_range = 4.0f * M_PI; const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) - int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; - int32_t encvaluesum = 0; - // go to motor zero phase for start_lock_duration to get ready to scan for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { @@ -776,6 +775,10 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } queue_voltage_timings(motor, voltage_magnitude, 0.0f); } + + int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; + int32_t encvaluesum = 0; + // scan forwards for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) { for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { @@ -789,6 +792,14 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; } + + float estEnc = scan_range / elec_rad_per_enc; + float adjustedCount = fabsf((int16_t)motor->encoder.encoder_timer->Instance->CNT)-init_enc_val; + if(fabsf(adjustedCount-estEnc)/estEnc > motor->encoder.encoder_calib_range) + { + motor->error = ERROR_ENCODER_CPR_OUT_OF_RANGE; + return false; + } // check direction if ((int16_t)motor->encoder.encoder_timer->Instance->CNT > init_enc_val + 8) { // motor same dir as encoder diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 9045b1d9..deb89f3c 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -50,6 +50,7 @@ typedef enum { ERROR_SPIN_UP_TIMEOUT, ERROR_DRV_FAULT, ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, + ERROR_ENCODER_CPR_OUT_OF_RANGE } Error_t; // Note: these should be sorted from lowest level of control to @@ -119,6 +120,7 @@ typedef struct { int32_t encoder_offset; int32_t encoder_state; int32_t motor_dir; // 1/-1 for fwd/rev alignment to encoder. + float encoder_calib_range; float phase; float pll_pos; float pll_vel; From 4d0366f8dff70e8b356c7a0b3f539407685531f1 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Mon, 19 Feb 2018 20:01:45 -0500 Subject: [PATCH 5/7] Change the calibration distance & number of steps for accuracy --- Firmware/MotorControl/low_level.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index d0a7ff5f..afc63053 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -762,9 +762,9 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h // TODO: add check_timing bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { static const float start_lock_duration = 1.0f; - static const int num_steps = 1024; + static const int num_steps = 1024*4; static const float dt_step = 1.0f / 500.0f; - static const float scan_range = 4.0f * M_PI; + static const float scan_range = 16.0f * M_PI; const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) // go to motor zero phase for start_lock_duration to get ready to scan From 95d1d3ff0fa4b0edc3e24e8aaafb17aeaaa1855c Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 21 Feb 2018 18:46:51 -0500 Subject: [PATCH 6/7] Fix fabsf bracket, make variable names more descriptive --- Firmware/MotorControl/low_level.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index afc63053..f3624eef 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -793,9 +793,9 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; } - float estEnc = scan_range / elec_rad_per_enc; - float adjustedCount = fabsf((int16_t)motor->encoder.encoder_timer->Instance->CNT)-init_enc_val; - if(fabsf(adjustedCount-estEnc)/estEnc > motor->encoder.encoder_calib_range) + float expected_encoder_delta = scan_range / elec_rad_per_enc; + float actual_encoder_delta_abs = fabsf((int16_t)motor->encoder.encoder_timer->Instance->CNT-init_enc_val); + if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > motor->encoder.encoder_calib_range) { motor->error = ERROR_ENCODER_CPR_OUT_OF_RANGE; return false; From 430e4088754e76843da96579060aee9d8c762fe3 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 23 Feb 2018 12:48:27 -0800 Subject: [PATCH 7/7] default calib speed a bit faster --- Firmware/MotorControl/low_level.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index f3624eef..40ac40ad 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -762,7 +762,7 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h // TODO: add check_timing bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { static const float start_lock_duration = 1.0f; - static const int num_steps = 1024*4; + static const int num_steps = 1024*2; static const float dt_step = 1.0f / 500.0f; static const float scan_range = 16.0f * M_PI; const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?)