From 92b0c21a4ea762b45a0dc8c3a784a84ec5395de4 Mon Sep 17 00:00:00 2001 From: Josh Pieper Date: Wed, 13 Jan 2021 18:02:16 -0500 Subject: [PATCH] Fix the sign of the feedforward velocity component This requires some moderately extensive backward compatibility changes so that old hardware which has a new firmware applied won't result in an inherently unstable system. --- fw/bldc_servo.cc | 2 +- fw/bldc_servo.h | 2 +- fw/moteus_hw.h | 11 +++++++++- lib/python/moteus/moteus_tool.py | 35 +++++++++++++++++++++++++++++++- 4 files changed, 46 insertions(+), 4 deletions(-) diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc index 3aa99334..ef421ebd 100644 --- a/fw/bldc_servo.cc +++ b/fw/bldc_servo.cc @@ -1422,7 +1422,7 @@ class BldcServo::Impl { const float q_V = (config_.feedforward_scale * ( - i_q_A * motor_.resistance_ohm - + i_q_A * motor_.resistance_ohm + status_.velocity * motor_.v_per_hz / motor_.unwrapped_position_scale)) + pid_q_.Apply(status_.q_A, i_q_A, 0.0f, 0.0f, kRateHz); diff --git a/fw/bldc_servo.h b/fw/bldc_servo.h index 2da95430..5dbacf13 100644 --- a/fw/bldc_servo.h +++ b/fw/bldc_servo.h @@ -140,7 +140,7 @@ class BldcServo { float derate_temperature = 50.0f; float fault_temperature = 75.0f; - float feedforward_scale = 1.0f; + float feedforward_scale = 0.5f; float velocity_threshold = 0.09f; float position_derate = 0.02f; diff --git a/fw/moteus_hw.h b/fw/moteus_hw.h index 8a575922..26add2e5 100644 --- a/fw/moteus_hw.h +++ b/fw/moteus_hw.h @@ -214,6 +214,15 @@ constexpr int kCompatibleHwRev[] = { #define MOTEUS_MODEL_NUMBER ((MOTEUS_HW_REV) << 8 | 0x00) -#define MOTEUS_FIRMWARE_VERSION 0x000100 +#define MOTEUS_FIRMWARE_VERSION 0x000101 + +// Version history: + +// # 0x0101 # +// +// * Fixed the calculation of feedforward voltage to have the correct +// sign for the velocity component. Previous firmwares, +// inappropriately applied a negative feedforward term, which +// counteracted rotation instead of being an actual feedforward. } diff --git a/lib/python/moteus/moteus_tool.py b/lib/python/moteus/moteus_tool.py index abf03e5d..2c72e0b0 100644 --- a/lib/python/moteus/moteus_tool.py +++ b/lib/python/moteus/moteus_tool.py @@ -47,12 +47,45 @@ def __init__(self, old, new): self.old = old self.new = new - if new > 0x0100: + if new > 0x0101: raise RuntimeError("Firmware to be flashed has a newer version than we support") def fix_config(self, old_config): lines = old_config.split(b'\n') + items = dict([line.split(b' ') for line in lines if b' ' in line]) + + #### 0x0101 + # + # This version was the first to have the correct sign applied + # to the velocity component of servo.feedforward_scale. The + # old versions applied a negative value, which acted opposite + # to its desired intention. The default value of the scale + # was 1.0, which could cause velocity instability when used + # with the new correct algorithm. + # + # So, when upgrading, if the value was at its default, we move + # it to the new default value. If it had been modified in any + # way, we zero it out, which is safe, although it will result + # in decreased performance. + + if self.new >= 0x0101 and self.old <= 0x0100: + # If the old firmware had the default feedforward term of + # 1.0, then switch it to be the new default of 0.5. + if float(items[b'servo.feedforward_scale']) == 1.0: + items[b'servo.feedforward_scale'] = b'0.5' + print("Changing servo.feedforward_scale from 1.0 to 0.5 for version 0x0101") + else: + items[b'servo.feedforward_scale'] = b'0.0' + print("Changing servo.feedforward_scale to 0.0 for version 0x0101") + + if self.new <= 0x0100 and self.old >= 0x0101: + # To get back to identical behavior, we apply the inverse + # mapping. + if float(items[b'servo.feedforward_scale']) == 0.5: + items[b'servo.feedforward_scale'] = b'1.0' + print("Reverting servo.feedforward_scale from 0.5 to 1.0 for version 0x0101") + lines = [key + b' ' + value for key, value in items.items()] return b'\n'.join(lines)