From 0b1bf6f278413fe14e4c7dc4ac6795e198f34361 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 15 Apr 2018 00:38:39 -0700 Subject: [PATCH] implement circular vel pll tracking --- Firmware/MotorControl/encoder.cpp | 28 ++++++++++++++++++++-------- Firmware/MotorControl/encoder.hpp | 4 ++-- Firmware/MotorControl/utils.h | 9 +++++++++ tools/odrive/shell.py | 2 +- tools/odrive/tests.py | 12 ++++++------ tools/odrivetool | 4 ++-- 6 files changed, 40 insertions(+), 19 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index b5ae480e..7f573752 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -50,11 +50,20 @@ void Encoder::set_count(int32_t count) { // Disable interrupts to make a critical section to avoid race condition uint32_t prim = __get_PRIMASK(); __disable_irq(); + + // Update states + shadow_count_ = count; + pos_estimate_ = (float)count; + count_in_cpr_ = mod(count, config_.cpr); + pos_cpr = (float)count_in_cpr_; + // Offset and state must be shifted by the same amount offset_ += count - shadow_count_; - shadow_count_ = count; //TODO FIXME + offset_ = mod(offset_, config_.cpr); + + //Write hardware last hw_config_.timer->Instance->CNT = count; - pll_pos_ = (float)count; + __set_PRIMASK(prim); } @@ -215,16 +224,19 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp // run pll (for now pll is in units of encoder counts) // Predict current pos pos_estimate_ += current_meas_period * pll_vel_; - pll_pos_ += current_meas_period * pll_vel_; + pos_cpr += current_meas_period * pll_vel_; // discrete phase detector - // float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pll_pos_)); - float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pll_pos_)); + float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_)); + float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr)); + delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr)); // pll feedback - pll_pos_ += current_meas_period * pll_kp_ * delta_pos; - pll_vel_ += current_meas_period * pll_ki_ * delta_pos; + pos_estimate_ += current_meas_period * pll_kp_ * delta_pos; + pos_cpr += current_meas_period * pll_kp_ * delta_pos_cpr; + pos_cpr = fmodf_pos(pos_cpr, (float)(config_.cpr)); + pll_vel_ += current_meas_period * pll_ki_ * delta_pos_cpr; // Assign output arguments - if (pos_estimate) *pos_estimate = pll_pos_; + if (pos_estimate) *pos_estimate = pos_estimate_; if (vel_estimate) *vel_estimate = pll_vel_; if (phase_output) *phase_output = phase_; return true; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 57194e75..39139e1f 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -55,7 +55,7 @@ public: int32_t offset_ = 0; float phase_ = 0.0f; // [rad] float pos_estimate_ = 0.0f; // [rad] - float pll_pos_ = 0.0f; // [rad] + float pos_cpr = 0.0f; // [rad] float pll_vel_ = 0.0f; // [rad/s] float pll_kp_ = 0.0f; // [rad/s / rad] float pll_ki_ = 0.0f; // [(rad/s^2) / rad] @@ -71,7 +71,7 @@ public: make_protocol_property("offset", &offset_), make_protocol_property("phase", &phase_), make_protocol_property("pos_estimate", &pos_estimate_), - make_protocol_property("pll_pos", &pll_pos_), + make_protocol_property("pos_cpr", &pos_cpr), make_protocol_property("pll_vel", &pll_vel_), make_protocol_property("pll_kp", &pll_kp_), make_protocol_property("pll_ki", &pll_ki_), diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 158065a0..52e7f98b 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -7,6 +7,7 @@ extern "C" { #endif #include +#include /** * @brief Unique ID register address location @@ -94,6 +95,14 @@ static inline float wrap_pm_pi(float theta) { return wrap_pm(theta, M_PI); } +// like fmodf, but always positive +static inline float fmodf_pos(float x, float y) { + float out = fmodf(x, y); + if (out < 0.0f) + out += y; + return out; +} + // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta // as per the magnitude invariant clarke transform // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2 diff --git a/tools/odrive/shell.py b/tools/odrive/shell.py index af2e9432..19fee9dd 100644 --- a/tools/odrive/shell.py +++ b/tools/odrive/shell.py @@ -22,7 +22,7 @@ def print_help(args): print('Type "odrv0." and press ') print('This will present you with all the properties that you can reference') print('') - print('For example: "odrv0.motor0.encoder.pll_pos"') + print('For example: "odrv0.motor0.encoder.pos_estimate"') print('will print the current encoder position on motor 0') print('and "odrv0.motor0.pos_setpoint = 10000"') print('will send motor0 to 10000') diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index eb052947..de4c6d8a 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -342,13 +342,13 @@ class TestClosedLoopControl(AxisTest): time.sleep(0.001) test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) time.sleep(0.1) # give the PLL some time to settle - init_pos = axis_ctx.handle.encoder.pll_pos + init_pos = axis_ctx.handle.encoder.pos_estimate axis_ctx.handle.controller.set_pos_setpoint(init_pos+1000, 0, 0) time.sleep(0.5) - test_assert_eq(axis_ctx.handle.encoder.pll_pos, init_pos+1000, range=200) + test_assert_eq(axis_ctx.handle.encoder.pos_estimate, init_pos+1000, range=200) axis_ctx.handle.controller.set_pos_setpoint(init_pos-1000, 0, 0) time.sleep(0.5) - test_assert_eq(axis_ctx.handle.encoder.pll_pos, init_pos-1000, range=400) + test_assert_eq(axis_ctx.handle.encoder.pos_estimate, init_pos-1000, range=400) logger.debug("closed loop control: test vel_limit") axis_ctx.handle.controller.set_pos_setpoint(50000, 0, 0) @@ -551,7 +551,7 @@ class TestVelCtrlVsPosCtrl(DualAxisTest): # Turn to some position logger.debug("using {} as driver against load, vel=100000...".format(driver_ctx.name)) set_limits(driver_ctx, logger, vel_limit=100000, current_limit=50) - init_pos = driver_ctx.handle.encoder.pll_pos + init_pos = driver_ctx.handle.encoder.pos_estimate driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) for _ in range(int(4000/5)): @@ -563,7 +563,7 @@ class TestVelCtrlVsPosCtrl(DualAxisTest): logger.debug("using {} as driver against load, vel=20000...".format(driver_ctx.name)) set_limits(driver_ctx, logger, vel_limit=20000, current_limit=50) - init_pos = driver_ctx.handle.encoder.pll_pos + init_pos = driver_ctx.handle.encoder.pos_estimate driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) #for _ in range(int(5*4000/5)): @@ -580,6 +580,6 @@ class TestVelCtrlVsPosCtrl(DualAxisTest): ## Turn to another position #logger.debug("controlling against load, vel=40000...") #set_limits(axis1_ctx, logger, vel_limit=40000, current_limit=20) - #init_pos = axis1_ctx.handle.encoder.pll_pos + #init_pos = axis1_ctx.handle.encoder.pos_estimate #axis1_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) #request_state(axis1_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) diff --git a/tools/odrivetool b/tools/odrivetool index 4afa2927..add4c0c6 100755 --- a/tools/odrivetool +++ b/tools/odrivetool @@ -100,8 +100,8 @@ try: # If you want to plot different values, change them here. # You can plot any number of values concurrently. - start_liveplotter(lambda: [my_odrive.motor0.encoder.pll_pos, - my_odrive.motor1.encoder.pll_pos]) + start_liveplotter(lambda: [my_odrive.motor0.encoder.pos_estimate, + my_odrive.motor1.encoder.pos_estimate]) elif args.command == 'drv-status': from odrive.utils import print_drv_regs