Skip to content

Commit

Permalink
Factor the position logic so that it can be unit tested
Browse files Browse the repository at this point in the history
  • Loading branch information
@jpieper
jpieper committed Apr 7, 2022
1 parent 6b1b683 commit 0b823df
Show file tree
Hide file tree
Showing 19 changed files with 1,073 additions and 707 deletions.
18 changes: 12 additions & 6 deletions fw/BUILD
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
# -*- python -*-

# Copyright 2018-2020 Josh Pieper, jjp@pobox.com.
# Copyright 2018-2022 Josh Pieper, jjp@pobox.com.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
Expand All @@ -26,16 +26,26 @@ COPTS = [
cc_library(
name = "common",
hdrs = [
"bldc_servo_position.h",
"bldc_servo_structs.h",
"ccm.h",
"error.h",
"foc.h",
"math.h",
"measured_hw_rev.h",
"pid.h",
"simple_pi.h",
"torque_model.h",
"stm32_i2c_timing.h",
],
srcs = [
"foc.cc",
],
deps = [
"@com_github_mjbots_mjlib//mjlib/base:limit",
"@com_github_mjbots_mjlib//mjlib/base:visitor",
"@com_github_mjbots_mjlib//mjlib/micro:atomic_event_queue",
"@com_github_mjbots_mjlib//mjlib/micro:error_code",
],
copts = COPTS,
)
Expand Down Expand Up @@ -83,7 +93,6 @@ MOTEUS_SOURCES = [
"bootloader.h",
"drv8323.h",
"drv8323.cc",
"error.h",
"error.cc",
"hardfault.s",
"firmware_info.h",
Expand All @@ -94,8 +103,6 @@ MOTEUS_SOURCES = [
"moteus_hw.h",
"millisecond_timer.h",
"motor_driver.h",
"pid.h",
"simple_pi.h",
"stm32_i2c.h",
"stm32_spi.h",
"stm32_serial.h",
Expand All @@ -112,13 +119,11 @@ MOTEUS_DEPS = [
":git_info",
"@com_github_mjbots_mjlib//mjlib/base:assert",
"@com_github_mjbots_mjlib//mjlib/base:inplace_function",
"@com_github_mjbots_mjlib//mjlib/base:limit",
"@com_github_mjbots_mjlib//mjlib/base:windowed_average",
"@com_github_mjbots_mjlib//mjlib/micro:async_exclusive",
"@com_github_mjbots_mjlib//mjlib/micro:async_stream",
"@com_github_mjbots_mjlib//mjlib/micro:callback_table",
"@com_github_mjbots_mjlib//mjlib/micro:command_manager",
"@com_github_mjbots_mjlib//mjlib/micro:error_code",
"@com_github_mjbots_mjlib//mjlib/micro:persistent_config",
"@com_github_mjbots_mjlib//mjlib/micro:pool_ptr",
"@com_github_mjbots_mjlib//mjlib/micro:telemetry_manager",
Expand Down Expand Up @@ -176,6 +181,7 @@ genrule(
cc_test(
name = "test",
srcs = [
"test/bldc_servo_position_test.cc",
"test/foc_test.cc",
"test/math_test.cc",
"test/stm32_i2c_timing_test.cc",
Expand Down
96 changes: 13 additions & 83 deletions fw/bldc_servo.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include "mjlib/base/assert.h"
#include "mjlib/base/windowed_average.h"

#include "fw/bldc_servo_position.h"
#include "fw/foc.h"
#include "fw/math.h"
#include "fw/moteus_hw.h"
Expand Down Expand Up @@ -1778,89 +1779,15 @@ class BldcServo::Impl {
float max_torque_Nm,
float feedforward_Nm,
float velocity) MOTEUS_CCM_ATTRIBUTE {
// We go to some lengths in our conversions to and from
// control_position so as to avoid converting a float directly to
// an int64, which calls out to a system library that is pretty
// slow.

if (!std::isnan(data->position)) {
status_.control_position =
static_cast<int64_t>(65536ll * 65536ll) *
static_cast<int64_t>(
static_cast<int32_t>(motor_scale16_ * data->position));
data->position = std::numeric_limits<float>::quiet_NaN();
} else if (!status_.control_position) {
status_.control_position = status_.unwrapped_position_raw;
}

auto velocity_command = velocity;

// This limits our usable velocity to 20kHz modulo the position
// scale at a 40kHz switching frequency. 1.2 million RPM should
// be enough for anybody?
status_.control_position =
(*status_.control_position +
65536ll * static_cast<int32_t>(
(65536.0f * motor_scale16_ * velocity_command) /
rate_config_.rate_hz));

if (std::isfinite(config_.max_position_slip)) {
const int64_t current_position = status_.unwrapped_position_raw;
const int64_t slip =
static_cast<int64_t>(65536ll * 65536ll) *
static_cast<int32_t>(motor_scale16_ * config_.max_position_slip);

const int64_t error =
current_position - *status_.control_position;
if (error < -slip) {
*status_.control_position = current_position + slip;
}
if (error > slip) {
*status_.control_position = current_position - slip;
}
}

bool hit_limit = false;

const auto saturate = [&](auto value, auto compare) MOTEUS_CCM_ATTRIBUTE {
if (std::isnan(value)) { return; }
const auto limit_value = (
static_cast<int64_t>(65536ll * 65536ll) *
static_cast<int64_t>(
static_cast<int32_t>(motor_scale16_ * value)));
if (compare(*status_.control_position, limit_value)) {
status_.control_position = limit_value;
hit_limit = true;
}
};
saturate(position_config_.position_min, [](auto l, auto r) { return l < r; });
saturate(position_config_.position_max, [](auto l, auto r) { return l > r; });

if (!std::isnan(data->stop_position)) {
const int64_t stop_position_raw =
static_cast<int64_t>(65536ll * 65536ll) *
static_cast<int64_t>(
static_cast<int32_t>(motor_scale16_ * data->stop_position));

auto sign = [](auto value) MOTEUS_CCM_ATTRIBUTE -> float {
if (value < 0) { return -1.0f; }
if (value > 0) { return 1.0f; }
return 0.0f;
};
if (sign(*status_.control_position -
stop_position_raw) * velocity_command > 0.0f) {
// We are moving away from the stop position. Force it to be
// there and zero out our velocity command.
status_.control_position = stop_position_raw;
data->velocity = 0.0f;
hit_limit = true;
}
}

if (hit_limit) {
// We have hit a limit. Assume a velocity of 0.
velocity_command = 0.0f;
}
const float velocity_command =
BldcServoPosition::UpdateCommand(
&status_,
&config_,
&position_config_,
motor_scale16_,
rate_config_.rate_hz,
data,
velocity);

// At this point, our control position and velocity are known.

Expand Down Expand Up @@ -2089,6 +2016,7 @@ class BldcServo::Impl {
Motor motor_;
Config config_;
PositionConfig position_config_;

TIM_TypeDef* timer_ = nullptr;
volatile uint32_t* timer_sr_ = nullptr;
volatile uint32_t* timer_cr1_ = nullptr;
Expand Down Expand Up @@ -2175,8 +2103,10 @@ class BldcServo::Impl {

float torque_constant_ = 0.01f;
int32_t position_constant_ = 0;

// 65536.0f / unwrapped_position_scale_
float motor_scale16_ = 0;

float adc_scale_ = 0.0f;
float adjusted_pwm_comp_off_ = 0.0f;
float adjusted_max_power_W_ = 0.0f;
Expand Down
Loading

0 comments on commit 0b823df

Please sign in to comment.