From 0e85e45718e8e9d4d6532d0b75374759cf7e0388 Mon Sep 17 00:00:00 2001 From: Josh Pieper Date: Mon, 13 May 2024 10:24:50 -0400 Subject: [PATCH] Support scaling the ilimit on a per-command basis This makes it possible to switch between velocity and position mode when a non-zero ilimit is configured. --- docs/reference.md | 102 +++++++++++++---------- fw/bldc_servo.cc | 3 +- fw/bldc_servo_structs.h | 2 + fw/board_debug.cc | 6 +- fw/moteus_controller.cc | 12 ++- fw/pid.h | 13 +-- lib/cpp/mjbots/moteus/moteus_multiplex.h | 2 + lib/cpp/mjbots/moteus/moteus_protocol.h | 21 +++++ lib/python/moteus/moteus.py | 11 +++ 9 files changed, 119 insertions(+), 53 deletions(-) diff --git a/docs/reference.md b/docs/reference.md index 7b2b17ae..819407f6 100644 --- a/docs/reference.md +++ b/docs/reference.md @@ -897,41 +897,49 @@ If specified and not-NaN, then the control mode will temporarily be in the "fixed voltage" mode, regardless of the current setting of `servo.fixed_voltage_mode`. -### 0x030 - Proportional torque ### +#### 0x02b - Ki ilimit scale #### + +Mode: Read/write + +When in Position mode, shrink the integral term's windup limit by the +given factor. Integral types are applied as for PWM. If unspecified, +1.0 is used. + +#### 0x030 - Proportional torque #### Mode: Read This reports the torque contribution from the proportional term in the PID controller. -### 0x031 - Integral torque ### +#### 0x031 - Integral torque #### Mode: Read This reports the torque contribution from the integral term in the PID controller. -### 0x032 - Derivative torque ### +#### 0x032 - Derivative torque #### Mode: Read This reports the torque contribution from the derivative term in the PID controller. -### 0x033 - Feedforward torque ### +#### 0x033 - Feedforward torque #### Mode: Read This reports the feedforward contribution in the PID controller. -### 0x034 - Total control torque ### +#### 0x034 - Total control torque #### Mode: Read This reports the total commanded torque from the position mode controller. This is also reported in 0x03a. -### 0x038 - Control Position ### +#### 0x038 - Control Position #### Mode: Read @@ -940,7 +948,7 @@ that is valid. When velocity or acceleration limiting is enabled, the control position will follow the desired limits to achieve the command position. -### 0x039 - Control Velocity ### +#### 0x039 - Control Velocity #### Mode: Read @@ -949,32 +957,32 @@ is valid. When velocity or acceleration limiting is enabled, the control velocity will follow the desired limits to achieve the control position and velocity. -### 0x03a - Control Torque ### +#### 0x03a - Control Torque #### Mode: Read The torque commanded by the control loop. This is the same as 0x034. -### 0x03b - Position Error ### +#### 0x03b - Position Error #### Mode: Read The current sensed position minus the control position. -### 0x03c - Velocity Error ### +#### 0x03c - Velocity Error #### Mode: Read The current sensed velocity minus the control velocity. -### 0x03d - Torque Error ### +#### 0x03d - Torque Error #### Mode: Read The current sensed torque minus the control torque. -### 0x040 - Stay within lower bound ### +#### 0x040 - Stay within lower bound #### Mode: Read/write @@ -986,7 +994,7 @@ feedforward torque is applied. When outside this bound, the PID controller is used to force the position back to the bound. If unspecified, 0.0 is used. -### 0x041 - Stay within upper bound ### +#### 0x041 - Stay within upper bound #### Mode: Read/write @@ -998,76 +1006,80 @@ a feedforward torque is applied. When outside this bound, the PID controller is used to force the position back to the bound. If unspecified, 0.0 is used. -### 0x042 - Feedforward torque ### +#### 0x042 - Feedforward torque #### A shadow of the 0x022 register. -### 0x043 - Kp scale ### +#### 0x043 - Kp scale #### A shadow of the 0x023 register. -### 0x044 - Kd scale ### +#### 0x044 - Kd scale #### A shadow of the 0x024 register. -### 0x045 - Maximum torque ### +#### 0x045 - Maximum torque #### A shadow of the 0x025 register. -### 0x046 - Watchdog timeout ### +#### 0x046 - Watchdog timeout #### A shadow of the 0x027 register. -### 0x050 - Encoder 0 Position ### +#### 0x047 - Ki ilimit scale #### + +A shadow of the 0x02b register. + +#### 0x050 - Encoder 0 Position #### Mode: Read only Reports the current filtered position of the encoder configured in slot 0. -### 0x051 - Encoder 0 Velocity ### +#### 0x051 - Encoder 0 Velocity #### Mode: Read only Reports the current filtered velocity of the encoder configured in slot 0. -### 0x052 - Encoder 1 Position ### +#### 0x052 - Encoder 1 Position #### Mode: Read only Reports the current filtered position of the encoder configured in slot 1. -### 0x053 - Encoder 1 Velocity ### +#### 0x053 - Encoder 1 Velocity #### Mode: Read only Reports the current filtered velocity of the encoder configured in slot 1. -### 0x054 - Encoder 2 Position ### +#### 0x054 - Encoder 2 Position #### Mode: Read only Reports the current filtered position of the encoder configured in slot 2. -### 0x055 - Encoder 2 Velocity ### +#### 0x055 - Encoder 2 Velocity #### Mode: Read only Reports the current filtered velocity of the encoder configured in slot 2. -### 0x058 - Encoder Validity ### +#### 0x058 - Encoder Validity #### Mode: Read only Returns a bitfield, where bit 0 indicates whether encoder 0 is active, bit 1 indicates whether encoder 1 is active, etc. -### 0x05c - Aux1 GPIO Command ### +#### 0x05c - Aux1 GPIO Command #### Mode: Read/write @@ -1075,7 +1087,7 @@ The current output command for any GPIOs configured as an output on aux1 as a bitfield. Not all bits may be used, as bit 0 is always for pin 1, whether or not it is configured as a GPIO output. -### 0x05d - Aux2 GPIO Command ### +#### 0x05d - Aux2 GPIO Command #### Mode: Read/write @@ -1083,7 +1095,7 @@ The current output command for any GPIOs configured as an output on aux2 as a bitfield. Not all bits may be used, as bit 0 is always for pin 1, whether or not it is configured as a GPIO output. -### 0x05e - Aux1 GPIO Status ### +#### 0x05e - Aux1 GPIO Status #### Mode: Read only @@ -1091,7 +1103,7 @@ The current input value of any GPIOs configured as an input on aux1 as a bitfield. Not all bits may be used, as bit 0 is always for pin 1, whether or not it is configured as a GPIO input. -### 0x05f - Aux2 GPIO Status ### +#### 0x05f - Aux2 GPIO Status #### Mode: Read only @@ -1099,7 +1111,7 @@ The current input value of any GPIOs configured as an input on aux2 as a bitfield. Not all bits may be used, as bit 0 is always for pin 1, whether or not it is configured as a GPIO input. -### 0x060/0x064 - Aux1 Analog Inputs ### +#### 0x060/0x064 - Aux1 Analog Inputs #### Mode: Read only @@ -1108,7 +1120,7 @@ registers are associated with pins 1-5, regardless of whether they are configured as an analog input. Each value is scaled as a PWM from 0 to 1. -### 0x068/0x06c - Aux2 Analog Inputs ### +#### 0x068/0x06c - Aux2 Analog Inputs #### Mode: Read only @@ -1117,7 +1129,7 @@ registers are associated with pins 1-5, regardless of whether they are configured as an analog input. Each value is scaled as a PWM from 0 to 1. -### 0x070 - Millisecond Counter ### +#### 0x070 - Millisecond Counter #### Mode: Read only @@ -1125,7 +1137,7 @@ Increments once per millisecond. It wraps at the maximum value for the queried type to the minimum value for that type. For floating point types, it counts integers from 0 to 8388608. -### 0x071 - Clock Trim ### +#### 0x071 - Clock Trim #### Mode: Read/write @@ -1139,14 +1151,14 @@ microcontroller, including CAN communication. Thus setting this to a non-zero value may prevent future CAN communications. -### 0x100 - Model Number ### +#### 0x100 - Model Number #### Name: Model Number Mode: Read only This returns a 32 bit model number. -### 0x101 - Firmware Version ### +#### 0x101 - Firmware Version #### Mode: Read only @@ -1154,14 +1166,14 @@ This returns a 32 bit firmware version, encoded bytewise as major.minor.micro. i.e. 0x010304 is version 1.3.4 -### 0x102 - Register map version ### +#### 0x102 - Register map version #### Mode: Read only This returns a number that indicates how to interpret all registers. -### 0x110 - Multiplex ID ### +#### 0x110 - Multiplex ID #### Name: Multiplex ID Mode: Configurable @@ -1170,7 +1182,7 @@ This controls the primary ID used to access the device over the multiplex RS485 bus. It can only be between 1 and 127. (0 is reserved as the broadcast address). -### 0x120 - 0x122 - Serial Number ### +#### 0x120 - 0x122 - Serial Number #### Name: Serial Number Mode: Read only @@ -1178,7 +1190,7 @@ Mode: Read only This returns a 96 bit serial number, least significant word first. -### 0x130 - Set Output Nearest ### +#### 0x130 - Set Output Nearest #### Mode: Write only @@ -1186,14 +1198,14 @@ When sent, this causes the servo to select a whole number of internal motor rotations so that the final position is as close to the given position as possible. -### 0x131 - Set Output Exact ### +#### 0x131 - Set Output Exact #### Mode: Write only When sent, the servo will force the output position to be the exact specified value. -### 0x132 - Require Reindex ### +#### 0x132 - Require Reindex #### Mode: Write only @@ -1202,7 +1214,7 @@ position be re-located before control can begin. Regardless, the position will reset to an arbitrary value consistent with the current encoder settings. -### 0x133 - Recapture Position and Velocity ### +#### 0x133 - Recapture Position and Velocity #### Mode: Write only @@ -1214,7 +1226,7 @@ current applied torque is 0, either because of an in-place maximum torque limit of zero, or because of an in-place kp and kd scale of zero. -### 0x140 - Driver Fault 1 ### +#### 0x140 - Driver Fault 1 #### Mode: Read only @@ -1223,7 +1235,7 @@ for fault register 1. Up to 16 bits may be set. This will only be non-zero if the current mode is fault (1) and the fault code is 33 (motor driver fault). -### 0x141 - Driver Fault 2 ### +#### 0x141 - Driver Fault 2 #### Mode: Read only. @@ -1347,6 +1359,8 @@ Each optional element consists of a prefix character followed by a value. Permi constant for the duration of this command - `d` - kd scale: the configured kd value is multiplied by this constant for the duration of this command +- `i` - ki ilimit scale: the configured ilimit value is multiplied by + this constant for the duration of this command - `s` - stop position: when a non-zero velocity is given, motion stops when the control position reaches this value. - `f` - feedforward torque in Nm diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc index e1627c54..e9da36c6 100644 --- a/fw/bldc_servo.cc +++ b/fw/bldc_servo.cc @@ -1938,7 +1938,7 @@ class BldcServo::Impl { PID::ApplyOptions apply_options; apply_options.kp_scale = 0.0f; apply_options.kd_scale = data->kd_scale; - apply_options.ki_scale = 0.0f; + apply_options.ilimit_scale = 0.0f; ISR_DoPositionCommon(sin_cos, &zero_velocity, apply_options, config_.timeout_max_torque_Nm, @@ -1949,6 +1949,7 @@ class BldcServo::Impl { PID::ApplyOptions apply_options; apply_options.kp_scale = data->kp_scale; apply_options.kd_scale = data->kd_scale; + apply_options.ilimit_scale = data->ilimit_scale; ISR_DoPositionCommon(sin_cos, data, apply_options, data->max_torque_Nm, data->feedforward_Nm, data->velocity); diff --git a/fw/bldc_servo_structs.h b/fw/bldc_servo_structs.h index 34c3ce0d..e44a3d56 100644 --- a/fw/bldc_servo_structs.h +++ b/fw/bldc_servo_structs.h @@ -310,6 +310,7 @@ struct BldcServoCommandData { float kp_scale = 1.0f; float kd_scale = 1.0f; + float ilimit_scale = 1.0f; float velocity_limit = std::numeric_limits::quiet_NaN(); float accel_limit = std::numeric_limits::quiet_NaN(); @@ -353,6 +354,7 @@ struct BldcServoCommandData { a->Visit(MJ_NVP(feedforward_Nm)); a->Visit(MJ_NVP(kp_scale)); a->Visit(MJ_NVP(kd_scale)); + a->Visit(MJ_NVP(ilimit_scale)); a->Visit(MJ_NVP(velocity_limit)); a->Visit(MJ_NVP(accel_limit)); a->Visit(MJ_NVP(fixed_voltage_override)); diff --git a/fw/board_debug.cc b/fw/board_debug.cc index 6016a230..2ba109c8 100644 --- a/fw/board_debug.cc +++ b/fw/board_debug.cc @@ -87,6 +87,10 @@ bool ParseOptions(BldcServo::CommandData* command, base::Tokenizer* tokenizer, command->kd_scale = value; break; } + case 'i': { + command->ilimit_scale = value; + break; + } case 's': { command->stop_position = value; break; @@ -590,7 +594,7 @@ class BoardDebug::Impl { // We default to no timeout for debug commands. command.timeout_s = std::numeric_limits::quiet_NaN(); - if (!ParseOptions(&command, &tokenizer, "pdsftavo")) { + if (!ParseOptions(&command, &tokenizer, "pdisftavo")) { WriteMessage(response, "ERR unknown option\r\n"); return; } diff --git a/fw/moteus_controller.cc b/fw/moteus_controller.cc index c7523fff..10264afa 100644 --- a/fw/moteus_controller.cc +++ b/fw/moteus_controller.cc @@ -259,6 +259,7 @@ enum class Register { kCommandVelocityLimit = 0x028, kCommandAccelLimit = 0x029, kCommandFixedVoltageOverride = 0x02a, + kCommandIlimitScale = 0x02b, kPositionKp = 0x030, kPositionKi = 0x031, @@ -280,6 +281,7 @@ enum class Register { kStayWithinKdScale = 0x044, kStayWithinMaxTorque = 0x045, kStayWithinTimeout = 0x046, + kStayWithinIlimitScale = 0x047, kEncoder0Position = 0x050, kEncoder0Velocity = 0x051, @@ -621,6 +623,11 @@ class MoteusController::Impl : public multiplex::MicroServer::Server { command_.kd_scale = ReadPwm(value); return 0; } + case Register::kCommandIlimitScale: + case Register::kStayWithinIlimitScale: { + command_.ilimit_scale = ReadPwm(value); + return 0; + } case Register::kStayWithinLower: { command_.bounds_min = ReadPosition(value); return 0; @@ -861,7 +868,10 @@ class MoteusController::Impl : public multiplex::MicroServer::Server { case Register::kStayWithinKdScale: { return ScalePwm(command_.kd_scale, type); } - + case Register::kCommandIlimitScale: + case Register::kStayWithinIlimitScale: { + return ScalePwm(command_.ilimit_scale, type); + } case Register::kPositionKp: { return ScaleTorque(bldc_.status().pid_position.p, type); } diff --git a/fw/pid.h b/fw/pid.h index b19d4255..147b11da 100644 --- a/fw/pid.h +++ b/fw/pid.h @@ -100,7 +100,7 @@ class PID { struct ApplyOptions { float kp_scale = 1.0f; float kd_scale = 1.0f; - float ki_scale = 1.0f; + float ilimit_scale = 1.0f; ApplyOptions() {} }; @@ -144,10 +144,11 @@ class PID { state_->integral += to_update_i; - if (state_->integral > config_->ilimit) { - state_->integral = config_->ilimit; - } else if (state_->integral < -config_->ilimit) { - state_->integral = -config_->ilimit; + const float ilimit = config_->ilimit * apply_options.ilimit_scale; + if (state_->integral > ilimit) { + state_->integral = ilimit; + } else if (state_->integral < -ilimit) { + state_->integral = -ilimit; } state_->p = apply_options.kp_scale * config_->kp * state_->error; @@ -155,7 +156,7 @@ class PID { state_->pd = state_->p + state_->d; state_->command = config_->sign * - (state_->pd + apply_options.ki_scale * state_->integral); + (state_->pd + state_->integral); return state_->command; } diff --git a/lib/cpp/mjbots/moteus/moteus_multiplex.h b/lib/cpp/mjbots/moteus/moteus_multiplex.h index 125951a0..2f676fce 100644 --- a/lib/cpp/mjbots/moteus/moteus_multiplex.h +++ b/lib/cpp/mjbots/moteus/moteus_multiplex.h @@ -450,6 +450,7 @@ class MultiplexParser { static constexpr int8_t kCurrent = 8; static constexpr int8_t kTheta = 9; static constexpr int8_t kPower = 10; + static constexpr int8_t kAcceleration = 11; double ReadConcrete(Resolution res, int8_t concrete_type) { #ifndef ARDUINO @@ -468,6 +469,7 @@ class MultiplexParser { 1.0, 0.1, 0.001, // kCurrent 1.0 / 127.0 * M_PI, 1.0 / 32767.0 * M_PI, 1.0 / 2147483647.0 * M_PI, // kTheta 10.0, 0.05, 0.0001, // kPower + 0.05, 0.001, 0.00001, // kAcceleration }; #ifndef ARDUINO diff --git a/lib/cpp/mjbots/moteus/moteus_protocol.h b/lib/cpp/mjbots/moteus/moteus_protocol.h index b6b97cd2..ed62ecc8 100644 --- a/lib/cpp/mjbots/moteus/moteus_protocol.h +++ b/lib/cpp/mjbots/moteus/moteus_protocol.h @@ -138,6 +138,10 @@ enum Register : uint16_t { kCommandPositionMaxTorque = 0x025, kCommandStopPosition = 0x026, kCommandTimeout = 0x027, + kCommandVelocityLimit = 0x028, + kCommandAccelLimit = 0x029, + kCommandFixedVoltageOverride = 0x02a, + kCommandIlimitScale = 0x02b, kPositionKp = 0x030, kPositionKi = 0x031, @@ -159,6 +163,7 @@ enum Register : uint16_t { kCommandStayWithinKdScale = 0x044, kCommandStayWithinPositionMaxTorque = 0x045, kCommandStayWithinTimeout = 0x046, + kCommandStayWithinIlimitScale = 0x047, kEncoder0Position = 0x050, kEncoder0Velocity = 0x051, @@ -596,6 +601,9 @@ struct Query { { R::kCommandPositionMaxTorque, 1, MP::kTorque, }, { R::kCommandStopPosition, 1, MP::kPosition, }, { R::kCommandTimeout, 1, MP::kTime, }, + { R::kCommandVelocityLimit, 1, MP::kVelocity, }, + { R::kCommandAccelLimit, 1, MP::kAcceleration, }, + { R::kCommandFixedVoltageOverride, 1, MP::kVoltage }, { R::kPositionKp, 5, MP::kTorque, }, // { R::kPositionKi, 1, MP::kTorque, }, @@ -617,6 +625,7 @@ struct Query { // { R::kCommandStayWithinKdScale, 1, MP::kPwm, }, { R::kCommandStayWithinPositionMaxTorque, 1, MP::kTorque, }, { R::kCommandStayWithinTimeout, 1, MP::kTime, }, + { R::kCommandStayWithinIlimitScale, 1, MP::kPwm }, { R::kEncoder0Position, 1, MP::kPosition, }, { R::kEncoder0Velocity, 1, MP::kVelocity, }, @@ -810,6 +819,7 @@ struct PositionMode { double velocity_limit = NaN; double accel_limit = NaN; double fixed_voltage_override = NaN; + double ilimit_scale = 1.0; }; struct Format { @@ -824,6 +834,7 @@ struct PositionMode { Resolution velocity_limit = kIgnore; Resolution accel_limit = kIgnore; Resolution fixed_voltage_override = kIgnore; + Resolution ilimit_scale = kIgnore; }; static uint8_t Make(WriteCanData* frame, @@ -847,6 +858,7 @@ struct PositionMode { format.velocity_limit, format.accel_limit, format.fixed_voltage_override, + format.ilimit_scale, }; WriteCombiner combiner( frame, 0x00, @@ -888,6 +900,9 @@ struct PositionMode { frame->WriteVoltage(command.fixed_voltage_override, format.fixed_voltage_override); } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.ilimit_scale, format.ilimit_scale); + } return 0; } }; @@ -1004,6 +1019,7 @@ struct StayWithinMode { double kd_scale = 1.0; double maximum_torque = 0.0; double watchdog_timeout = NaN; + double ilimit_scale = 1.0; }; struct Format { @@ -1014,6 +1030,7 @@ struct StayWithinMode { Resolution kd_scale = kIgnore; Resolution maximum_torque = kIgnore; Resolution watchdog_timeout = kIgnore; + Resolution ilimit_scale = kIgnore; }; static uint8_t Make(WriteCanData* frame, @@ -1031,6 +1048,7 @@ struct StayWithinMode { format.kd_scale, format.maximum_torque, format.watchdog_timeout, + format.ilimit_scale, }; WriteCombiner combiner( @@ -1061,6 +1079,9 @@ struct StayWithinMode { if (combiner.MaybeWrite()) { frame->WriteTime(command.watchdog_timeout, format.watchdog_timeout); } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.ilimit_scale, format.ilimit_scale); + } return 0; } }; diff --git a/lib/python/moteus/moteus.py b/lib/python/moteus/moteus.py index 0698cbb0..e0ea4e3a 100644 --- a/lib/python/moteus/moteus.py +++ b/lib/python/moteus/moteus.py @@ -200,6 +200,7 @@ class Register(enum.IntEnum): COMMAND_VELOCITY_LIMIT = 0x028 COMMAND_ACCEL_LIMIT = 0x029 COMMAND_FIXED_VOLTAGE_OVERRIDE = 0x02a + COMMAND_ILIMIT_SCALE = 0x02b POSITION_KP = 0x030 POSITION_KI = 0x031 @@ -221,6 +222,7 @@ class Register(enum.IntEnum): COMMAND_WITHIN_KD_SCALE = 0x044 COMMAND_WITHIN_MAX_TORQUE = 0x045 COMMAND_WITHIN_TIMEOUT = 0x046 + COMMAND_WITHIN_ILIMIT_SCALE = 0x047 ENCODER_0_POSITION = 0x050 ENCODER_0_VELOCITY = 0x051 @@ -331,6 +333,7 @@ class PositionResolution: velocity_limit = mp.F32 accel_limit = mp.F32 fixed_voltage_override = mp.F32 + ilimit_scale = mp.F32 class VFOCResolution: @@ -899,6 +902,7 @@ def make_position(self, velocity_limit=None, accel_limit=None, fixed_voltage_override=None, + ilimit_scale=None, query=False, query_override=None): """Return a moteus.Command structure with data necessary to send a @@ -920,6 +924,7 @@ def make_position(self, pr.velocity_limit if velocity_limit is not None else mp.IGNORE, pr.accel_limit if accel_limit is not None else mp.IGNORE, pr.fixed_voltage_override if fixed_voltage_override is not None else mp.IGNORE, + pr.ilimit_scale if ilimit_scale is not None else mp.IGNORE ] data_buf = io.BytesIO() @@ -954,6 +959,8 @@ def make_position(self, writer.write_accel(accel_limit, pr.accel_limit) if combiner.maybe_write(): writer.write_voltage(fixed_voltage_override, pr.fixed_voltage_override) + if combiner.maybe_write(): + writer.write_voltage(ilimit_scale, pr.ilimit_scale) self._format_query(query, query_override, data_buf, result) @@ -1109,6 +1116,7 @@ def make_stay_within( maximum_torque=None, stop_position=None, watchdog_timeout=None, + ilimit_scale=None, query=False, query_override=None): """Return a moteus.Command structure with data necessary to send a @@ -1126,6 +1134,7 @@ def make_stay_within( pr.kd_scale if kd_scale is not None else mp.IGNORE, pr.maximum_torque if maximum_torque is not None else mp.IGNORE, pr.watchdog_timeout if watchdog_timeout is not None else mp.IGNORE, + pr.ilimit_scale if ilimit_scale is not None else mp.IGNORE, ] data_buf = io.BytesIO() @@ -1153,6 +1162,8 @@ def make_stay_within( writer.write_torque(maximum_torque, pr.maximum_torque) if combiner.maybe_write(): writer.write_time(watchdog_timeout, pr.watchdog_timeout) + if combiner.maybe_write(): + writer.write_pwm(ilimit_scale, pr.ilimit_scale) self._format_query(query, query_override, data_buf, result)