From 44ae4473d2fb287710403a3df4e3926c5197376b Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 16:01:26 -0700 Subject: [PATCH 01/23] add cppcrc --- src/MODULE.bazel | 7 +++++++ src/extlibs/cppcrc.BUILD | 9 +++++++++ 2 files changed, 16 insertions(+) create mode 100644 src/extlibs/cppcrc.BUILD diff --git a/src/MODULE.bazel b/src/MODULE.bazel index 323fd7de00..c3ba4db1bc 100644 --- a/src/MODULE.bazel +++ b/src/MODULE.bazel @@ -210,6 +210,13 @@ http_archive( url = "https://github.com/analogdevicesinc/TMC-API/archive/0cd695fab6d43ceb121af4b8608e5d92b14e1ce9.tar.gz", ) +http_archive( + name = "cppcrc", + build_file = "@//extlibs:cppcrc.BUILD", + strip_prefix = "cppcrc-6360eee1c8966d32b2552ce156b135ec6a3235f1", + url = "https://github.com/DarrenLevine/cppcrc/archive/6360eee1c8966d32b2552ce156b135ec6a3235f1.zip", +) + ############################################## # Register our Toolchains ############################################## diff --git a/src/extlibs/cppcrc.BUILD b/src/extlibs/cppcrc.BUILD new file mode 100644 index 0000000000..80a1586127 --- /dev/null +++ b/src/extlibs/cppcrc.BUILD @@ -0,0 +1,9 @@ +# Description: +# A very small, fast, header-only, C++ library for generating CRCs +# https://github.com/DarrenLevine/cppcrc/tree/main + +cc_library( + name = "cppcrc", + hdrs = ["cppcrc.h"], + visibility = ["//visibility:public"], +) \ No newline at end of file From f1367a8da96f54cf08d8a44de900d99c9b9879b1 Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 16:08:54 -0700 Subject: [PATCH 02/23] protobuf changes for fault messaging --- src/proto/robot_status_msg.proto | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/proto/robot_status_msg.proto b/src/proto/robot_status_msg.proto index ac153b4f85..a326a6deec 100644 --- a/src/proto/robot_status_msg.proto +++ b/src/proto/robot_status_msg.proto @@ -81,6 +81,7 @@ enum ErrorCode enum MotorFault { + /*********** TMC Faults ************/ // Refer to Trinamic 6100 datasheet for precise definitions of motor faults. // https://trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC6100_datasheet_Rev1.00.pdf // (Section 5.1, p22-23) @@ -99,6 +100,20 @@ enum MotorFault PHASE_W_SHORT_COUNTER_DETECTED = 12; PHASE_W_SHORT_TO_GND_DETECTED = 13; PHASE_W_SHORT_TO_VS_DETECTED = 14; + + /*********** STSPIN Faults ************/ + NO_FAULT = 15; + DURATION = 16; + OVER_VOLT = 17; + UNDER_VOLT = 18; + OVER_TEMP = 19; + START_UP = 20; + SPEED_FDBK = 21; + OVER_CURR = 22; + SW_ERROR = 23; + SAMPLE_FAULT = 24; + OVERCURR_SW = 25; + DP_FAULT = 26; } message DriveUnit From f59fa1e7e7c82448e94f734ff27f138a4601143d Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 16:14:33 -0700 Subject: [PATCH 03/23] gpio resources --- src/software/embedded/gpio/BUILD | 18 ++++++++++++++++ src/software/embedded/gpio/gpio.cpp | 21 +++++++++++++++++++ src/software/embedded/{ => gpio}/gpio.h | 12 +++++++++++ .../embedded/{ => gpio}/gpio_char_dev.cpp | 2 +- .../embedded/{ => gpio}/gpio_char_dev.h | 4 ++-- 5 files changed, 54 insertions(+), 3 deletions(-) create mode 100644 src/software/embedded/gpio/BUILD create mode 100644 src/software/embedded/gpio/gpio.cpp rename src/software/embedded/{ => gpio}/gpio.h (51%) rename src/software/embedded/{ => gpio}/gpio_char_dev.cpp (97%) rename src/software/embedded/{ => gpio}/gpio_char_dev.h (92%) diff --git a/src/software/embedded/gpio/BUILD b/src/software/embedded/gpio/BUILD new file mode 100644 index 0000000000..6b79ab53d6 --- /dev/null +++ b/src/software/embedded/gpio/BUILD @@ -0,0 +1,18 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "gpio", + srcs = [ + "gpio.cpp", + "gpio_char_dev.cpp", + ], + hdrs = [ + "gpio.h", + "gpio_char_dev.h", + ], + deps = [ + "//software/logger", + "//software/logger:network_logger", + "//software/util/make_enum", + ], +) \ No newline at end of file diff --git a/src/software/embedded/gpio/gpio.cpp b/src/software/embedded/gpio/gpio.cpp new file mode 100644 index 0000000000..7b609fd349 --- /dev/null +++ b/src/software/embedded/gpio/gpio.cpp @@ -0,0 +1,21 @@ +#include "software/embedded/gpio/gpio.h" + +#include +#include + +bool Gpio::pollValue(GpioState state, std::chrono::milliseconds timeout_ms) +{ + const auto start_time = std::chrono::system_clock::now(); + while (getValue() != state) + { + std::this_thread::sleep_for(std::chrono::microseconds(100)); + + const auto current_time = std::chrono::system_clock::now(); // Use steady clock + const auto elapsed_time = current_time - start_time; + if (elapsed_time > timeout_ms) + { + return false; + } + } + return true; +} \ No newline at end of file diff --git a/src/software/embedded/gpio.h b/src/software/embedded/gpio/gpio.h similarity index 51% rename from src/software/embedded/gpio.h rename to src/software/embedded/gpio/gpio.h index c07e15c239..ae5e6c9ac1 100644 --- a/src/software/embedded/gpio.h +++ b/src/software/embedded/gpio/gpio.h @@ -1,5 +1,6 @@ #pragma once +#include #include "software/util/make_enum/make_enum.hpp" MAKE_ENUM(GpioState, LOW, HIGH); @@ -22,4 +23,15 @@ class Gpio * Get the current state of the gpio */ virtual GpioState getValue() = 0; + + /** + * Polls the GPIO until it reaches the requested state or a timeout occurs + * + * @param state The GPIO state to wait for + * @param timeout_ms Maximum wait duration in milliseconds + * + * @return true if the GPIO reached the requested state within the timeout, + * false if timed out before the GPIO reached the requested state + */ + bool pollValue(GpioState state, std::chrono::milliseconds timeout_ms); }; diff --git a/src/software/embedded/gpio_char_dev.cpp b/src/software/embedded/gpio/gpio_char_dev.cpp similarity index 97% rename from src/software/embedded/gpio_char_dev.cpp rename to src/software/embedded/gpio/gpio_char_dev.cpp index 772a0f6f65..1ddf9b4f51 100644 --- a/src/software/embedded/gpio_char_dev.cpp +++ b/src/software/embedded/gpio/gpio_char_dev.cpp @@ -1,4 +1,4 @@ -#include "software/embedded/gpio_char_dev.h" +#include "software/embedded/gpio/gpio_char_dev.h" #include diff --git a/src/software/embedded/gpio_char_dev.h b/src/software/embedded/gpio/gpio_char_dev.h similarity index 92% rename from src/software/embedded/gpio_char_dev.h rename to src/software/embedded/gpio/gpio_char_dev.h index 87e69d3141..0b013c0171 100644 --- a/src/software/embedded/gpio_char_dev.h +++ b/src/software/embedded/gpio/gpio_char_dev.h @@ -4,7 +4,7 @@ #include -#include "software/embedded/gpio.h" +#include "software/embedded/gpio/gpio.h" /** * GPIO with the character device interface @@ -24,7 +24,7 @@ class GpioCharDev : public Gpio * @param char_dev_path The path to the gpio character device */ GpioCharDev(uint32_t gpio_number, GpioDirection direction, GpioState state, - std::string char_dev_path = "/dev/gpiochip0"); + std::string char_dev_path = "/dev/gpiochip4"); /** * Destructor From ba6ac9ef28df11436632cf980905e9127e168e6d Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 16:29:17 -0700 Subject: [PATCH 04/23] stspin motor controller implementation --- src/software/embedded/motor_controller/BUILD | 67 +++ .../embedded/motor_controller/motor_board.h | 11 + .../motor_controller/motor_controller.h | 57 +++ .../motor_fault_indicator.cpp | 14 + .../motor_controller/motor_fault_indicator.h | 38 ++ .../stspin_motor_controller.cpp | 463 ++++++++++++++++++ .../stspin_motor_controller.h | 142 ++++++ .../embedded/motor_controller/stspin_types.h | 93 ++++ 8 files changed, 885 insertions(+) create mode 100644 src/software/embedded/motor_controller/BUILD create mode 100644 src/software/embedded/motor_controller/motor_board.h create mode 100644 src/software/embedded/motor_controller/motor_controller.h create mode 100644 src/software/embedded/motor_controller/motor_fault_indicator.cpp create mode 100644 src/software/embedded/motor_controller/motor_fault_indicator.h create mode 100644 src/software/embedded/motor_controller/stspin_motor_controller.cpp create mode 100644 src/software/embedded/motor_controller/stspin_motor_controller.h create mode 100644 src/software/embedded/motor_controller/stspin_types.h diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD new file mode 100644 index 0000000000..ffe0cc3997 --- /dev/null +++ b/src/software/embedded/motor_controller/BUILD @@ -0,0 +1,67 @@ +package(default_visibility = ["//visibility:public"]) +cc_library( + name = "motor_index", + hdrs = ["motor_index.h"], + deps = [ + "//software/util/make_enum", + ], +) +cc_library( + name = "motor_board", + hdrs = ["motor_board.h"], + deps = [ + "//software/util/make_enum", + ], +) +cc_library( + name = "motor_controller", + srcs = [ + "stspin_motor_controller.cpp", + "tmc_motor_controller.cpp", + ], + hdrs = [ + "motor_controller.h", + "stspin_motor_controller.h", + "tmc_motor_controller.h", + ], + deps = [ + ":motor_board", + ":motor_fault_indicator", + ":motor_index", + ":stspin_types", + "//proto/message_translation:tbots_protobuf", + "//software/embedded:spi_utils", + "//software/embedded/gpio", + "//software/logger", + "//software/physics:euclidean_to_wheel", + "@cppcrc", + "@trinamic", + ], +) +cc_library( + name = "motor_fault_indicator", + srcs = ["motor_fault_indicator.cpp"], + hdrs = ["motor_fault_indicator.h"], + deps = [ + ":motor_board", + "//proto:tbots_cc_proto", + ], +) +cc_library( + name = "stspin_types", + hdrs = ["stspin_types.h"], +) +cc_binary( + name = "stspin_motor_controller_test", + srcs = ["stspin_motor_controller_test.cpp"], + linkopts = [ + "-lpthread", + "-lrt", + ], + deps = [ + ":motor_controller", + "//software/logger", + "//software/logger:network_logger", + "@boost//:program_options", + ], +) \ No newline at end of file diff --git a/src/software/embedded/motor_controller/motor_board.h b/src/software/embedded/motor_controller/motor_board.h new file mode 100644 index 0000000000..5a8bd32bb7 --- /dev/null +++ b/src/software/embedded/motor_controller/motor_board.h @@ -0,0 +1,11 @@ +#pragma once + +#include "software/util/make_enum/make_enum.hpp" + +MAKE_ENUM(MotorBoard, TRINAMIC, STSPIN); + +#ifdef TRINAMIC_MOTOR_BOARD +constexpr MotorBoard MOTOR_BOARD = MotorBoard::TRINAMIC; +#elif STSPIN_MOTOR_BOARD +constexpr MotorBoard MOTOR_BOARD = MotorBoard::STSPIN; +#endif \ No newline at end of file diff --git a/src/software/embedded/motor_controller/motor_controller.h b/src/software/embedded/motor_controller/motor_controller.h new file mode 100644 index 0000000000..ee4bdbec87 --- /dev/null +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -0,0 +1,57 @@ +#pragma once + +#include "software/embedded/motor_controller/motor_fault_indicator.h" +#include "software/embedded/motor_controller/motor_index.h" +#include "software/physics/euclidean_to_wheel.h" + +/** + * Abstract interface for motor controllers. + * + * Implementations provide hardware-specific setup, fault reporting, + * and velocity control for the robot's motors. + */ +class MotorController +{ + public: + virtual ~MotorController() = default; + + /** + * Initializes the motor controller hardware and any internal state. + */ + virtual void setup() = 0; + + /** + * Resets the underlying motor controller hardware. + */ + virtual void reset() = 0; + + /** + * Returns the current fault state for the given motor. + * + * @param motor the motor to inspect + * @return a MotorFaultIndicator indicating the faults for that motor + */ + virtual const MotorFaultIndicator& checkFaults(MotorIndex motor) = 0; + + /** + * Reads the current velocity and writes a new target velocity for a motor. + * + * @param motor the motor to command + * @param target_velocity the desired target velocity for the motor in mechanical RPM + * @return the current measured velocity of the motor in mechanical RPM + */ + virtual int readThenWriteVelocity(MotorIndex motor, int target_velocity) = 0; + + /** + * Updates the motor controller with the target local velocity of the robot + * in Euclidean space. + * + * @param target_euclidean_velocity the target local velocity of the robot + */ + virtual void updateEuclideanVelocity(EuclideanSpace_t target_euclidean_velocity) = 0; + + /** + * Immediately disables all motors. + */ + virtual void immediatelyDisable() = 0; +}; \ No newline at end of file diff --git a/src/software/embedded/motor_controller/motor_fault_indicator.cpp b/src/software/embedded/motor_controller/motor_fault_indicator.cpp new file mode 100644 index 0000000000..514a1e7606 --- /dev/null +++ b/src/software/embedded/motor_controller/motor_fault_indicator.cpp @@ -0,0 +1,14 @@ +#include "software/embedded/motor_controller/motor_fault_indicator.h" + +MotorFaultIndicator::MotorFaultIndicator() : drive_enabled(true) {} +MotorFaultIndicator::MotorFaultIndicator( + const bool drive_enabled, + const std::unordered_set& motor_faults) + : drive_enabled(drive_enabled), faults(motor_faults) +{ +} + +bool MotorFaultIndicator::requiresReset() const +{ + return !drive_enabled || faults.find(TbotsProto::MotorFault::RESET) != faults.end(); +} \ No newline at end of file diff --git a/src/software/embedded/motor_controller/motor_fault_indicator.h b/src/software/embedded/motor_controller/motor_fault_indicator.h new file mode 100644 index 0000000000..aacfe9718f --- /dev/null +++ b/src/software/embedded/motor_controller/motor_fault_indicator.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +#include "proto/robot_status_msg.pb.h" + +/** + * Holds motor fault information for a particular motor and whether any fault has + * caused the motor to be disabled. + */ +struct MotorFaultIndicator +{ + bool drive_enabled; + std::unordered_set faults; + + /** + * Construct a default indicator of no faults and running motors. + */ + MotorFaultIndicator(); + + /** + * Construct an indicator with faults and whether the motor is enabled. + * + * @param drive_enabled true if the motor is enabled, false if disabled due to a + * motor fault + * @param motor_faults a set of faults associated with this motor + */ + MotorFaultIndicator(bool drive_enabled, + const std::unordered_set& motor_faults); + + /** + * Checks if the motor requires a reset due to being disabled or + * having a reset fault. + * + * @return true if the motor requires a reset, false otherwise + */ + bool requiresReset() const; +}; \ No newline at end of file diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp new file mode 100644 index 0000000000..5836cf42f1 --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -0,0 +1,463 @@ +#include "software/embedded/motor_controller/stspin_motor_controller.h" + +#include + +#include +#include +#include + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wconversion" +#include "cppcrc.h" +#pragma GCC diagnostic pop + +#include "proto/message_translation/tbots_protobuf.h" +#include "software/embedded/gpio/gpio_char_dev.h" +#include "software/embedded/motor_controller/stspin_types.h" +#include "software/embedded/spi_utils.h" +#include "software/logger/logger.h" + +// AUTOSAR variant of CRC-8 +// (https://reveng.sourceforge.io/crc-catalogue/all.htm#crc.cat.crc-8-autosar) +using Crc8Autosar = crc_utils::crc; + +StSpinMotorController::StSpinMotorController( + const robot_constants::RobotConstants& robot_constants) + : robot_constants_(robot_constants), + reset_gpio_(std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, + GpioState::HIGH)) +{ + for (const MotorIndex motor : driveMotors()) + { + openSpiFileDescriptor(motor); + } +} + +void StSpinMotorController::setup() +{ + reset(); + + for (const MotorIndex motor : reflective_enum::values()) + { + motor_status_[motor] = MotorStatus(); + motor_status_[motor].enabled = true; + } + + for (const MotorIndex motor : driveMotors()) + { + sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = SPEED_PID_PROPORTIONAL_GAIN, + .ki = SPEED_PID_INTEGRAL_GAIN}); + } +} + +void StSpinMotorController::reset() +{ + reset_gpio_->setValue(GpioState::LOW); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + reset_gpio_->setValue(GpioState::HIGH); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); +} + +const MotorFaultIndicator& StSpinMotorController::checkFaults(const MotorIndex motor) +{ + return motor_status_.at(motor).faults; +} + +void StSpinMotorController::updateFaults(const MotorIndex motor, + const uint16_t fault_flags) +{ + MotorStatus& motor_status = motor_status_.at(motor); + + if (motor_status.fault_flags == fault_flags) + { + // No change in faults + return; + } + + motor_status.fault_flags = fault_flags; + MotorFaultIndicator& motor_faults = motor_status.faults; + motor_faults.drive_enabled = true; + motor_faults.faults.clear(); + + if (fault_flags == 0) + { + // No faults + return; + } + + std::ostringstream oss; + oss << "======= Faults For Motor " << motor << "=======\n"; + + if (fault_flags & static_cast(StSpinFaultCode::DURATION)) + { + oss << "DURATION: FOC rate too high\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::DURATION); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::OVER_VOLT)) + { + oss << "OVER_VOLT: Over voltage\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::OVER_VOLT); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::UNDER_VOLT)) + { + oss << "UNDER_VOLT: Under voltage\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::UNDER_VOLT); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::OVER_TEMP)) + { + oss << "OVER_TEMP: Over temperature\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::OVER_TEMP); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::START_UP)) + { + oss << "START_UP: Start up failed\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::START_UP); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::SPEED_FDBK)) + { + oss << "SPEED_FDBK: Speed feedback fault\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::SPEED_FDBK); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::OVER_CURR)) + { + oss << "OVER_CURR: Over current\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::OVER_CURR); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::SW_ERROR)) + { + oss << "SW_ERROR: Software error\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::SW_ERROR); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::SAMPLE_FAULT)) + { + oss << "SAMPLE_FAULT: Sample fault for testing purposes\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::SAMPLE_FAULT); + } + + if (fault_flags & static_cast(StSpinFaultCode::OVERCURR_SW)) + { + oss << "OVERCURR_SW: Software over current\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::OVERCURR_SW); + motor_faults.drive_enabled = false; + } + + if (fault_flags & static_cast(StSpinFaultCode::DP_FAULT)) + { + oss << "DP_FAULT: Driver protection fault\n"; + motor_faults.faults.insert(TbotsProto::MotorFault::DP_FAULT); + motor_faults.drive_enabled = false; + } + + LOG(WARNING) << oss.str(); //Consider moving this to a helper and also making the stream object persistent so we do not need to continuously recreate it (it is notoriously expensive). We could also consider another approach to formating this string as using streams in general is less than ideal. Or We can probably just use string::append + +} + +int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, + const int target_velocity) +{ + if (motor == MotorIndex::DRIBBLER) + { + return 0; + } + + const auto outgoing_frame = SetTargetSpeedFrame{ + .motor_enabled = motor_status_.at(motor).enabled, + .motor_target_speed_rpm = static_cast(target_velocity), + }; + + sendAndReceiveFrame(motor, outgoing_frame); + + return motor_status_.at(motor).speed; +} + +void StSpinMotorController::updateEuclideanVelocity( + EuclideanSpace_t target_euclidean_velocity) +{ + const Vector local_velocity(target_euclidean_velocity[1], + -target_euclidean_velocity[0]); + + if (local_velocity.length() <= 0.01) //Magic number + { + sendAndReceiveFrame( + MotorIndex::FRONT_LEFT, + SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + sendAndReceiveFrame( + MotorIndex::FRONT_RIGHT, + SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + sendAndReceiveFrame( + MotorIndex::BACK_RIGHT, + SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + sendAndReceiveFrame( + MotorIndex::BACK_LEFT, + SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + return; + } + + const Angle direction = local_velocity.orientation(); + + const Angle front_wheel_angle = + Angle::fromDegrees(robot_constants_.front_wheel_angle_deg); + const Angle back_wheel_angle = + Angle::fromDegrees(robot_constants_.back_wheel_angle_deg); + + const int16_t front_left_ks = static_cast( + MAX_SPEED_FEED_FORWARD_STATIC_GAIN * + std::abs((direction - Angle::quarter() + front_wheel_angle).sin())); + const int16_t front_right_ks = static_cast( + MAX_SPEED_FEED_FORWARD_STATIC_GAIN * + std::abs((direction + Angle::quarter() - front_wheel_angle).sin())); + const int16_t back_right_ks = static_cast( + MAX_SPEED_FEED_FORWARD_STATIC_GAIN * + std::abs((direction + Angle::quarter() + back_wheel_angle).sin())); + const int16_t back_left_ks = static_cast( + MAX_SPEED_FEED_FORWARD_STATIC_GAIN * + std::abs((direction - Angle::quarter() - back_wheel_angle).sin())); + + sendAndReceiveFrame(MotorIndex::FRONT_LEFT, + SetSpeedFeedForwardKsFrame{.ks = front_left_ks}); + sendAndReceiveFrame(MotorIndex::FRONT_RIGHT, + SetSpeedFeedForwardKsFrame{.ks = front_right_ks}); + sendAndReceiveFrame(MotorIndex::BACK_RIGHT, + SetSpeedFeedForwardKsFrame{.ks = back_right_ks}); + sendAndReceiveFrame(MotorIndex::BACK_LEFT, + SetSpeedFeedForwardKsFrame{.ks = back_left_ks}); +} + +void StSpinMotorController::immediatelyDisable() +{ + for (const MotorIndex motor : reflective_enum::values()) + { + motor_status_[motor].enabled = false; + readThenWriteVelocity(motor, 0); + } +} + +void StSpinMotorController::openSpiFileDescriptor(const MotorIndex motor) +{ + spi_fds_[motor] = open(SPI_PATHS.at(motor), O_RDWR); + CHECK(spi_fds_[motor] >= 0) + << "can't open device: " << motor << "error: " << strerror(errno); + + int ret = ioctl(spi_fds_[motor], SPI_IOC_WR_MODE32, &SPI_MODE); + CHECK(ret != -1) << "can't set spi mode for: " << motor + << "error: " << strerror(errno); + + ret = ioctl(spi_fds_[motor], SPI_IOC_WR_BITS_PER_WORD, &SPI_BITS); + CHECK(ret != -1) << "can't set bits_per_word for: " << motor + << "error: " << strerror(errno); + + ret = ioctl(spi_fds_[motor], SPI_IOC_WR_MAX_SPEED_HZ, &MAX_SPI_SPEED_HZ); + CHECK(ret != -1) << "can't set spi max speed hz for: " << motor + << "error: " << strerror(errno); +} + +void StSpinMotorController::sendAndReceiveFrame(const MotorIndex motor, + const OutgoingFrame& outgoing_frame) +{ + std::array tx{}; + std::array rx{}; + + populateTx(outgoing_frame, tx); + + spiTransfer(spi_fds_[motor], tx.data(), rx.data(), FRAME_LEN, SPI_SPEED_HZ); + + motor_status_[motor].frame_count++; + + // Frame integrity check + const uint8_t rx_crc = Crc8Autosar::calc(rx.data(), FRAME_LEN - 1); + if (rx[5] != rx_crc) + { + // Log RX buffer and expected vs. actual CRC + std::ostringstream oss; + oss << std::hex << std::uppercase << std::setfill('0') << "Expected CRC 0x" + << std::setw(2) << static_cast(rx[5]) << " but got 0x" << std::setw(2) + << static_cast(rx_crc) << ". RX: "; + for (const uint8_t byte : rx) + { + oss << "0x" << std::setw(2) << static_cast(byte) << " "; + } + + LOG(WARNING) << "Frame #" << motor_status_[motor].frame_count + << " received from motor " << motor << " failed integrity check. " + << oss.str(); + return; + } + + processRx(motor, rx); +} + +void StSpinMotorController::populateTx(const OutgoingFrame& outgoing_frame, + std::array& tx) +{ + std::visit( + [&](TFrame&& frame) + { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::NO_OP); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_TARGET_SPEED); + tx[1] = static_cast(frame.motor_enabled); + tx[2] = static_cast(0xFF & (frame.motor_target_speed_rpm >> 8)); + tx[3] = static_cast(0xFF & frame.motor_target_speed_rpm); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_TARGET_TORQUE); + tx[1] = static_cast(frame.motor_enabled); + tx[2] = static_cast(0xFF & (frame.motor_target_torque >> 8)); + tx[3] = static_cast(0xFF & frame.motor_target_torque); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_RESPONSE_TYPE); + tx[1] = static_cast(frame.response_type); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_PID_TORQUE_KP_KI); + tx[1] = static_cast(0xFF & (frame.kp >> 8)); + tx[2] = static_cast(0xFF & frame.kp); + tx[3] = static_cast(0xFF & (frame.ki >> 8)); + tx[4] = static_cast(0xFF & frame.ki); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_PID_FLUX_KP_KI); + tx[1] = static_cast(0xFF & (frame.kp >> 8)); + tx[2] = static_cast(0xFF & frame.kp); + tx[3] = static_cast(0xFF & (frame.ki >> 8)); + tx[4] = static_cast(0xFF & frame.ki); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_PID_SPEED_KP_KI); + tx[1] = static_cast(0xFF & (frame.kp >> 8)); + tx[2] = static_cast(0xFF & frame.kp); + tx[3] = static_cast(0xFF & (frame.ki >> 8)); + tx[4] = static_cast(0xFF & frame.ki); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_SPEED_FEED_FORWARD_KA_KV); + tx[1] = static_cast(0xFF & (frame.ka >> 8)); + tx[2] = static_cast(0xFF & frame.ka); + tx[3] = static_cast(0xFF & (frame.kv >> 8)); + tx[4] = static_cast(0xFF & frame.kv); + } + else if constexpr (std::is_same_v) + { + tx[0] = static_cast(StSpinOpcode::SET_SPEED_FEED_FORWARD_KS); + tx[1] = static_cast(0xFF & (frame.ks >> 8)); + tx[2] = static_cast(0xFF & frame.ks); + } + }, + outgoing_frame); + + tx[5] = Crc8Autosar::calc(tx.data(), FRAME_LEN - 1); +} + +void StSpinMotorController::processRx(const MotorIndex motor, + const std::array& rx) +{ + switch (static_cast(rx[0])) + { + case StSpinResponseType::SPEED_AND_FAULTS: + { + motor_status_[motor].speed = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + const uint16_t fault_flags = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + updateFaults(motor, fault_flags); + break; + } + case StSpinResponseType::IQ_AND_ID: + { + motor_status_[motor].iq = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].id = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + case StSpinResponseType::VQ_AND_VD: + { + motor_status_[motor].vq = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].vd = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + case StSpinResponseType::PHASE_CURRENT_AND_VOLTAGE: + { + motor_status_[motor].phase_current = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].phase_voltage = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + case StSpinResponseType::IQ_AND_IQ_REF: + { + motor_status_[motor].iq = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].iq_ref = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + case StSpinResponseType::ID_AND_ID_REF: + { + motor_status_[motor].id = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].id_ref = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + case StSpinResponseType::SPEED_AND_SPEED_REF: + { + motor_status_[motor].speed = + static_cast((static_cast(rx[1]) << 8) | rx[2]); + motor_status_[motor].speed_ref = + static_cast((static_cast(rx[3]) << 8) | rx[4]); + break; + } + } +} + +void StSpinMotorController::sendMotorStatusToPlotJuggler(const MotorIndex motor) +{ + for (const StSpinResponseType response_type : + {StSpinResponseType::SPEED_AND_SPEED_REF, StSpinResponseType::IQ_AND_IQ_REF, + StSpinResponseType::ID_AND_ID_REF, StSpinResponseType::SPEED_AND_FAULTS}) + { + sendAndReceiveFrame(motor, SetResponseTypeFrame{response_type}); + } + + const MotorStatus& motor_status = motor_status_.at(motor); + + LOG(PLOTJUGGLER) << *createPlotJugglerValue({ + {"speed_" + motor, motor_status.speed}, + {"speed_ref_" + motor, motor_status.speed_ref}, + {"iq_" + motor, motor_status.iq}, + {"iq_ref_" + motor, motor_status.iq_ref}, + {"id_" + motor, motor_status.id}, + {"id_ref_" + motor, motor_status.id_ref}, + }); +} \ No newline at end of file diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h new file mode 100644 index 0000000000..68f0c26092 --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -0,0 +1,142 @@ +#pragma once + +#include "software/embedded/gpio/gpio.h" +#include "software/embedded/motor_controller/motor_controller.h" +#include "software/embedded/motor_controller/motor_fault_indicator.h" +#include "software/embedded/motor_controller/motor_index.h" +#include "software/embedded/motor_controller/stspin_types.h" + +/** + * Motor controller for controlling our 6th generation STSPIN motor drivers. + * + * We communicate with our MDv6 boards over SPI in full-duplex mode using a custom + * frame-based protocol. This protocol is documented in the MDv6 firmware repo, which + * can be found at https://github.com/UBC-Thunderbots/MDv6_Firmware + */ +class StSpinMotorController : public MotorController +{ + public: + explicit StSpinMotorController( + const robot_constants::RobotConstants& robot_constants); + + void setup() override; + + void reset() override; + + const MotorFaultIndicator& checkFaults(MotorIndex motor) override; + + int readThenWriteVelocity(MotorIndex motor, int target_velocity) override; + + void updateEuclideanVelocity(EuclideanSpace_t target_euclidean_velocity) override; + + void immediatelyDisable() override; + + private: + using OutgoingFrame = + std::variant; + + // Length of frame (in number of bytes) + static constexpr unsigned int FRAME_LEN = 6; + + // clang-format off + static const inline std::unordered_map SPI_PATHS = { + {MotorIndex::FRONT_LEFT, "/dev/spidev0.2"}, + {MotorIndex::FRONT_RIGHT, "/dev/spidev0.4"}, + {MotorIndex::BACK_LEFT, "/dev/spidev0.3"}, + {MotorIndex::BACK_RIGHT, "/dev/spidev0.0"}, + }; + // clang-format on + + static constexpr uint32_t SPI_SPEED_HZ = 100000; + static constexpr uint32_t MAX_SPI_SPEED_HZ = 250000000; + static constexpr uint8_t SPI_BITS = 8; + static constexpr uint32_t SPI_MODE = 0; + + static constexpr int RESET_GPIO_PIN = 12; + + static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 700; + static constexpr int SPEED_PID_INTEGRAL_GAIN = 30; + + static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 750; + static constexpr int MIN_SPEED_FEED_FORWARD_STATIC_GAIN = 300; + robot_constants::RobotConstants robot_constants_; + + // SPI file descriptors + std::unordered_map spi_fds_; + + std::unique_ptr reset_gpio_; + + struct MotorStatus + { + unsigned int frame_count; + bool enabled; + MotorFaultIndicator faults; + uint16_t fault_flags; + int16_t speed; + int16_t speed_ref; + int16_t iq; + int16_t iq_ref; + int16_t id; + int16_t id_ref; + int16_t vq; + int16_t vd; + int16_t phase_current; + int16_t phase_voltage; + }; + + std::unordered_map motor_status_; + + /** + * Opens a SPI file descriptor for the given motor. + * + * @param motor the motor to open a SPI file descriptor for + */ + void openSpiFileDescriptor(MotorIndex motor); + + /** + * Transmits a frame to the given motor and receives a frame back over SPI. + * + * @param motor the motor to send the frame to + * @param outgoing_frame the outgoing frame to send to the motor + */ + void sendAndReceiveFrame(MotorIndex motor, const OutgoingFrame& outgoing_frame); + + /** + * Populates the transmit buffer with the data from an outgoing frame. + * + * @param outgoing_frame the outgoing frame to populate the transmit buffer with + * @param tx the transmit buffer to populate with the outgoing frame's data + */ + void populateTx(const OutgoingFrame& outgoing_frame, + std::array& tx); + + /** + * Processes a frame received from the given motor, caching any motor status + * the frame provides. + * + * @param motor the motor that the received frame corresponds to + * @param rx the receive buffer with the received frame to process + */ + void processRx(MotorIndex motor, const std::array& rx); + + /** + * Records that the given faults were raised for a given motor. + * + * @param motor the motor to update the faults of + * @param fault_flags the faults for the motor + */ + void updateFaults(MotorIndex motor, uint16_t fault_flags); + + /** + * Sends the target and measured speed and current (Iq and Id) for the + * specified motor to PlotJuggler. + * + * @param motor the motor to plot the status of + */ + void sendMotorStatusToPlotJuggler(MotorIndex motor); + + friend class StSpinMotorControllerTest; +}; \ No newline at end of file diff --git a/src/software/embedded/motor_controller/stspin_types.h b/src/software/embedded/motor_controller/stspin_types.h new file mode 100644 index 0000000000..82417386df --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_types.h @@ -0,0 +1,93 @@ +#pragma once + +#include + +enum class StSpinOpcode +{ + NO_OP = 0x00, + SET_TARGET_SPEED = 0x01, + SET_TARGET_TORQUE = 0x02, + SET_RESPONSE_TYPE = 0x03, + SET_PID_TORQUE_KP_KI = 0x04, + SET_PID_FLUX_KP_KI = 0x05, + SET_PID_SPEED_KP_KI = 0x06, + SET_SPEED_FEED_FORWARD_KA_KV = 0x07, + SET_SPEED_FEED_FORWARD_KS = 0x08, +}; + +enum class StSpinResponseType +{ + SPEED_AND_FAULTS = 0x01, + IQ_AND_ID = 0x02, + VQ_AND_VD = 0x03, + PHASE_CURRENT_AND_VOLTAGE = 0x04, + IQ_AND_IQ_REF = 0x05, + ID_AND_ID_REF = 0x06, + SPEED_AND_SPEED_REF = 0x07, +}; + +enum class StSpinFaultCode +{ + NO_FAULT = 0x0000, + DURATION = 0x0001, + OVER_VOLT = 0x0002, + UNDER_VOLT = 0x0004, + OVER_TEMP = 0x0008, + START_UP = 0x0010, + SPEED_FDBK = 0x0020, + OVER_CURR = 0x0040, + SW_ERROR = 0x0080, + SAMPLE_FAULT = 0x0100, + OVERCURR_SW = 0x0200, + DP_FAULT = 0x0400 +}; + +struct NoOpFrame +{ +}; + +struct SetTargetSpeedFrame +{ + bool motor_enabled; + int16_t motor_target_speed_rpm; +}; + +struct SetTargetTorqueFrame +{ + bool motor_enabled; + int16_t motor_target_torque; +}; + +struct SetResponseTypeFrame +{ + StSpinResponseType response_type; +}; + +struct SetPidTorqueKpKiFrame +{ + int16_t kp; + int16_t ki; +}; + +struct SetPidFluxKpKiFrame +{ + int16_t kp; + int16_t ki; +}; + +struct SetPidSpeedKpKiFrame +{ + int16_t kp; + int16_t ki; +}; + +struct SetSpeedFeedForwardKaKvFrame +{ + int16_t ka; + int16_t kv; +}; + +struct SetSpeedFeedForwardKsFrame +{ + int16_t ks; +}; \ No newline at end of file From d0a59a3525e7d838192947b5266d0a5fdfbd6a63 Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 16:48:56 -0700 Subject: [PATCH 05/23] motor.h and cpp edits --- src/software/embedded/services/BUILD | 33 +- src/software/embedded/services/motor.cpp | 1151 +++------------------- src/software/embedded/services/motor.h | 415 +------- 3 files changed, 172 insertions(+), 1427 deletions(-) diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index f763e4f8dc..dda1bccd1e 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -4,16 +4,22 @@ cc_library( name = "motor", srcs = ["motor.cpp"], hdrs = ["motor.h"], + defines = + select({ + "//software/embedded:build_trinamic": ["TRINAMIC_MOTOR_BOARD"], + "//software/embedded:build_stspin": ["STSPIN_MOTOR_BOARD"], + }), deps = [ "//proto:tbots_cc_proto", "//shared:robot_constants", - "//software/embedded:gpio", - "//software/embedded/constants", + "//software/embedded/gpio", + "//software/embedded/motor_controller", + "//software/embedded/motor_controller:motor_board", + "//software/embedded/motor_controller:motor_fault_indicator", "//software/logger", "//software/physics:euclidean_to_wheel", "//software/util/scoped_timespec_timer", "@eigen", - "@trinamic", ], ) @@ -29,23 +35,4 @@ cc_library( "@boost//:asio", "@boost//:filesystem", ], -) - -cc_binary( - name = "robot_auto_test", - srcs = ["robot_auto_test.cpp"], - linkopts = [ - "-lpthread", - "-lrt", - ], - deps = [ - ":motor", - ":power", - "//proto/message_translation:tbots_geometry", - "//proto/primitive:primitive_msg_factory", - "//shared:robot_constants", - "//software/embedded:primitive_executor", - "//software/logger", - "@trinamic", - ], -) +) \ No newline at end of file diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index e42ab8f3f3..f5f89e1447 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -1,373 +1,76 @@ #include "software/embedded/services/motor.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // Needed for mlockall() -#include // needed for getrusage -#include // needed for getrusage -#include // needed for sysconf(int name); - -#include - #include "proto/tbots_software_msgs.pb.h" +#include "software/embedded/motor_controller/motor_board.h" +#include "software/embedded/motor_controller/stspin_motor_controller.h" +#include "software/embedded/motor_controller/tmc_motor_controller.h" #include "software/logger/logger.h" -#include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" - -extern "C" -{ -#include "tmc/ic/TMC4671/TMC4671.h" -#include "tmc/ic/TMC4671/TMC4671_Register.h" -#include "tmc/ic/TMC4671/TMC4671_Variants.h" -#include "tmc/ic/TMC6100/TMC6100.h" -} - -// SPI Configs -static const uint32_t MAX_SPI_SPEED_HZ = 2000000; // 2 Mhz -static const uint32_t TMC6100_SPI_SPEED = 1000000; // 1 Mhz -static const uint32_t TMC4671_SPI_SPEED = 1000000; // 1 Mhz -static const uint8_t SPI_BITS = 8; -static const uint32_t SPI_MODE = 0x3u; -static const uint32_t NUM_RETRIES_SPI = 3; -static const uint32_t TMC_CMD_MSG_SIZE = 5; - -static double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; -static int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; - - -extern "C" -{ - // We need a static pointer here, because trinamic externs the following two - // SPI binding functions that we need to interface with their API. - // - // The motor service exclusively calls the trinamic API which triggers these - // functions. The motor service will set this variable in the constructor. - static MotorService* g_motor_service = NULL; - uint8_t tmc4671_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) - { - return g_motor_service->tmc4671ReadWriteByte(motor, data, last_transfer); - } - - uint8_t tmc6100_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) - { - return g_motor_service->tmc6100ReadWriteByte(motor, data, last_transfer); - } -} - -MotorService::MotorService(const robot_constants::RobotConstants& robot_constants, - int control_loop_frequency_hz) - : spi_demux_select_0_(setupGpio(SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO, - GpioDirection::OUTPUT, GpioState::LOW)), - spi_demux_select_1_(setupGpio(SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO, - GpioDirection::OUTPUT, GpioState::LOW)), - driver_control_enable_gpio_( - setupGpio(DRIVER_CONTROL_ENABLE_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), - reset_gpio_( - setupGpio(MOTOR_DRIVER_RESET_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), - robot_constants_(robot_constants), +MotorService::MotorService(const robot_constants::RobotConstants& robot_constants) + : robot_constants_(robot_constants), + motor_controller_(setupMotorController()), euclidean_to_four_wheel_(robot_constants), - motor_fault_detector_(0), - dribbler_ramp_rpm_(0), - tracked_motor_fault_start_time_(std::nullopt), + dribbler_target_rpm_(0), //placeholder, move to power_service + drive_motor_mps_per_rpm_(2 * M_PI * robot_constants.wheel_radius_meters / 60), num_tracked_motor_resets_(0) { - motorServiceInit(robot_constants, control_loop_frequency_hz); -} - -void MotorService::motorServiceInit( - const robot_constants::RobotConstants& robot_constants, int control_loop_frequency_hz) -{ - int ret = 0; - - /** - * Opens SPI File Descriptor - * - * @param motor_name The name of the motor the spi path is connected to - * @param chip_select Which chip select to use - */ -#define OPEN_SPI_FILE_DESCRIPTOR(motor_name, chip_select) \ - \ - file_descriptors_[chip_select] = open(SPI_PATHS[chip_select], O_RDWR); \ - CHECK(file_descriptors_[chip_select] >= 0) \ - << "can't open device: " << #motor_name << "error: " << strerror(errno); \ - \ - ret = ioctl(file_descriptors_[chip_select], SPI_IOC_WR_MODE32, &SPI_MODE); \ - CHECK(ret != -1) << "can't set spi mode for: " << #motor_name \ - << "error: " << strerror(errno); \ - \ - ret = ioctl(file_descriptors_[chip_select], SPI_IOC_WR_BITS_PER_WORD, &SPI_BITS); \ - CHECK(ret != -1) << "can't set bits_per_word for: " << #motor_name \ - << "error: " << strerror(errno); \ - \ - ret = ioctl(file_descriptors_[chip_select], SPI_IOC_WR_MAX_SPEED_HZ, \ - &MAX_SPI_SPEED_HZ); \ - CHECK(ret != -1) << "can't set spi max speed hz for: " << #motor_name \ - << "error: " << strerror(errno); - - OPEN_SPI_FILE_DESCRIPTOR(front_left, FRONT_LEFT_MOTOR_CHIP_SELECT) - OPEN_SPI_FILE_DESCRIPTOR(front_right, FRONT_RIGHT_MOTOR_CHIP_SELECT) - OPEN_SPI_FILE_DESCRIPTOR(back_left, BACK_LEFT_MOTOR_CHIP_SELECT) - OPEN_SPI_FILE_DESCRIPTOR(back_right, BACK_RIGHT_MOTOR_CHIP_SELECT) - OPEN_SPI_FILE_DESCRIPTOR(dribbler, DRIBBLER_MOTOR_CHIP_SELECT) - - // Make this instance available to the static functions above - g_motor_service = this; } -MotorService::~MotorService() {} - void MotorService::setup() { - const auto now = std::chrono::system_clock::now(); - long int total_duration_since_last_fault_s = 0; - if (tracked_motor_fault_start_time_.has_value()) - { - total_duration_since_last_fault_s = - std::chrono::duration_cast( - now - tracked_motor_fault_start_time_.value()) - .count(); - } - - if (tracked_motor_fault_start_time_.has_value() && - total_duration_since_last_fault_s < MOTOR_FAULT_TIME_THRESHOLD_S) - { - num_tracked_motor_resets_++; - } - else - { - tracked_motor_fault_start_time_ = std::make_optional(now); - num_tracked_motor_resets_ = 1; - } - - - if (tracked_motor_fault_start_time_.has_value() && - num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD_COUNT) - { - LOG(FATAL) << "In the last " << total_duration_since_last_fault_s - << "s, the motor board has reset " << num_tracked_motor_resets_ - << " times. Thunderloop crashing for safety."; - } - - prev_wheel_velocities_ = {0.0, 0.0, 0.0, 0.0}; - - // Clear faults by resetting all the chips on the motor board - reset_gpio_->setValue(GpioState::LOW); - usleep(MICROSECONDS_PER_MILLISECOND * 100); - - reset_gpio_->setValue(GpioState::HIGH); - usleep(MICROSECONDS_PER_MILLISECOND * 100); - - for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) - { - LOG(INFO) << "Clearing RESET for " << MOTOR_NAMES[motor]; - tmc6100_writeInt(motor, TMC6100_GSTAT, 0x00000001); - cached_motor_faults_[motor] = MotorFaultIndicator(); - encoder_calibrated_[motor] = false; - } - - // Drive Motor Setup - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; motor++) - { - setUpDriveMotor(motor); - } - - // Dribbler Motor Setup - startDriver(DRIBBLER_MOTOR_CHIP_SELECT); - checkDriverFault(DRIBBLER_MOTOR_CHIP_SELECT); - startController(DRIBBLER_MOTOR_CHIP_SELECT, true); - tmc4671_setTargetVelocity(DRIBBLER_MOTOR_CHIP_SELECT, 0); - checkEncoderConnections(); - - // calibrate the encoders - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; motor++) - { - startEncoderCalibration(motor); - } - - sleep(1); - - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; motor++) - { - endEncoderCalibration(motor); - } - - is_initialized_ = true; + prev_wheel_velocities_ = WheelSpace_t::Zero(); + target_wheel_velocities_ = WheelSpace_t::Zero(); + dribbler_target_rpm_ = 0; + motor_controller_->setup(); } -void MotorService::setUpDriveMotor(uint8_t motor) +void MotorService::reset() { - startDriver(motor); - checkDriverFault(motor); - // Start all the controllers as drive motor controllers - startController(motor, false); - tmc4671_setTargetVelocity(motor, 0); + motor_controller_->reset(); } -MotorService::MotorFaultIndicator MotorService::checkDriverFault(uint8_t motor) +std::unique_ptr MotorService::setupMotorController() { - bool drive_enabled = true; - std::unordered_set motor_faults; - - int gstat = tmc6100_readInt(motor, TMC6100_GSTAT); - std::bitset<32> gstat_bitset(gstat); - - if (gstat_bitset.any()) - { - LOG(WARNING) << "======= Faults For Motor " << std::to_string(motor) << "======="; - } - - if (gstat_bitset[0]) - { - LOG(WARNING) - << "Indicates that the IC has been reset. All registers have been cleared to reset values." - << "Attention: DRV_EN must be high to allow clearing reset"; - motor_faults.insert(TbotsProto::MotorFault::RESET); - } - - if (gstat_bitset[1]) - { - LOG(WARNING) - << "drv_otpw : Indicates, that the driver temperature has exceeded overtemperature prewarning-level." - << "No action is taken. This flag is latched."; - motor_faults.insert(TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE_PREWARNING); - } - - if (gstat_bitset[2]) + if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) { - LOG(WARNING) - << "drv_ot: Indicates, that the driver has been shut down due to overtemperature." - << "This flag can only be cleared when the temperature is below the limit again." - << "It is latched for information."; - motor_faults.insert(TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE); + return std::make_unique(); + } else { + return std::make_unique(robot_constants_); } - - if (gstat_bitset[3]) - { - LOG(WARNING) << "uv_cp: Indicates an undervoltage on the charge pump." - << "The driver is disabled during undervoltage." - << "This flag is latched for information."; - motor_faults.insert(TbotsProto::MotorFault::UNDERVOLTAGE_CHARGEPUMP); - drive_enabled = false; - } - - if (gstat_bitset[4]) - { - LOG(WARNING) << "shortdet_u: Short to GND detected on phase U." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_U_SHORT_COUNTER_DETECTED); - drive_enabled = false; - } - - if (gstat_bitset[5]) - { - LOG(WARNING) << "s2gu: Short to GND detected on phase U." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_U_SHORT_TO_GND_DETECTED); - drive_enabled = false; - } - - if (gstat_bitset[6]) - { - LOG(WARNING) << "s2vsu: Short to VS detected on phase U." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_U_SHORT_TO_VS_DETECTED); - drive_enabled = false; - } - - if (gstat_bitset[8]) - { - LOG(WARNING) << "shortdet_v: V short counter has triggered at least once."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_V_SHORT_COUNTER_DETECTED); - } - - if (gstat_bitset[9]) - { - LOG(WARNING) << "s2gv: Short to GND detected on phase V." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_V_SHORT_TO_GND_DETECTED); - drive_enabled = false; - } - - if (gstat_bitset[10]) - { - LOG(WARNING) << "s2vsv: Short to VS detected on phase V." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_V_SHORT_TO_VS_DETECTED); - drive_enabled = false; - } - - if (gstat_bitset[12]) - { - LOG(WARNING) << "shortdet_w: short counter has triggered at least once."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_COUNTER_DETECTED); - } - - if (gstat_bitset[13]) - { - LOG(WARNING) << "s2gw: Short to GND detected on phase W." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_TO_GND_DETECTED); - drive_enabled = false; - } - - if (gstat_bitset[14]) - { - LOG(WARNING) << "s2vsw: Short to VS detected on phase W." - << "The driver becomes disabled until flag becomes cleared."; - motor_faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_TO_VS_DETECTED); - drive_enabled = false; - } - - return MotorFaultIndicator(drive_enabled, motor_faults); } -TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_velocity_mps, - double front_right_velocity_mps, - double back_left_velocity_mps, - double back_right_velocity_mps, - double dribbler_rpm) +TbotsProto::MotorStatus MotorService::createMotorStatus( + const WheelSpace_t& current_wheel_velocities, const double dribbler_rpm) const { TbotsProto::MotorStatus motor_status; - cached_motor_faults_[motor_fault_detector_] = checkDriverFault(motor_fault_detector_); - - for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) + for (const MotorIndex motor : reflective_enum::values()) { - if (motor != DRIBBLER_MOTOR_CHIP_SELECT) + const MotorFaultIndicator& motor_faults = motor_controller_->checkFaults(motor); + + if (motor != MotorIndex::DRIBBLER) { TbotsProto::DriveUnit drive_status; - drive_status.set_enabled(cached_motor_faults_[motor].drive_enabled); + drive_status.set_enabled(motor_faults.drive_enabled); - for (const TbotsProto::MotorFault& fault : - cached_motor_faults_[motor].motor_faults) + for (const TbotsProto::MotorFault& fault : motor_faults.faults) { drive_status.add_motor_faults(fault); } - if (motor == FRONT_LEFT_MOTOR_CHIP_SELECT) + if (motor == MotorIndex::FRONT_LEFT) { *(motor_status.mutable_front_left()) = drive_status; } - if (motor == FRONT_RIGHT_MOTOR_CHIP_SELECT) + if (motor == MotorIndex::FRONT_RIGHT) { *(motor_status.mutable_front_right()) = drive_status; } - if (motor == BACK_LEFT_MOTOR_CHIP_SELECT) + if (motor == MotorIndex::BACK_LEFT) { *(motor_status.mutable_back_left()) = drive_status; } - if (motor == BACK_RIGHT_MOTOR_CHIP_SELECT) + if (motor == MotorIndex::BACK_RIGHT) { *(motor_status.mutable_back_right()) = drive_status; } @@ -376,9 +79,9 @@ TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_veloci { TbotsProto::DribblerStatus dribbler_status; dribbler_status.set_dribbler_rpm(static_cast(dribbler_rpm)); - dribbler_status.set_enabled(cached_motor_faults_[motor].drive_enabled); - for (const TbotsProto::MotorFault& fault : - cached_motor_faults_[motor].motor_faults) + dribbler_status.set_enabled(motor_faults.drive_enabled); + + for (const TbotsProto::MotorFault& fault : motor_faults.faults) { dribbler_status.add_motor_faults(fault); } @@ -388,124 +91,81 @@ TbotsProto::MotorStatus MotorService::updateMotorStatus(double front_left_veloci } motor_status.mutable_front_left()->set_wheel_velocity( - static_cast(front_left_velocity_mps)); + static_cast(current_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX])); motor_status.mutable_front_right()->set_wheel_velocity( - static_cast(front_right_velocity_mps)); + static_cast(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX])); motor_status.mutable_back_left()->set_wheel_velocity( - static_cast(back_left_velocity_mps)); + static_cast(current_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX])); motor_status.mutable_back_right()->set_wheel_velocity( - static_cast(back_right_velocity_mps)); - - motor_fault_detector_ = - static_cast((motor_fault_detector_ + 1) % NUM_MOTORS); + static_cast(current_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX])); return motor_status; } -TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor, - double time_elapsed_since_last_poll_s) +TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor_control, + const double time_elapsed_since_last_poll_s) { - bool encoders_calibrated = (encoder_calibrated_[FRONT_LEFT_MOTOR_CHIP_SELECT] || - encoder_calibrated_[FRONT_RIGHT_MOTOR_CHIP_SELECT] || - encoder_calibrated_[BACK_LEFT_MOTOR_CHIP_SELECT] || - encoder_calibrated_[BACK_RIGHT_MOTOR_CHIP_SELECT]); - - if (!encoders_calibrated) + if (anyMotorRequiresReset()) { - is_initialized_ = false; + LOG(INFO) << "Resetting motors due to fault indicators requiring reset"; + trackMotorReset(); + setup(); } - // checks if any motor has reset, sends a log message if so - for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) - { - if (requiresMotorReinit(motor)) - { - LOG(DEBUG) << "RESET DETECTED FOR MOTOR: " << MOTOR_NAMES[motor]; - is_initialized_ = false; - } - } + const Eigen::Vector4 target_wheel_rpms = + (target_wheel_velocities_ / drive_motor_mps_per_rpm_).cast(); - if (!is_initialized_) - { - LOG(INFO) << "MotorService re-initializing"; - setup(); - } + const Eigen::Vector4 current_wheel_rpms = { + motor_controller_->readThenWriteVelocity( + MotorIndex::FRONT_RIGHT, target_wheel_rpms[FRONT_RIGHT_WHEEL_SPACE_INDEX]), + motor_controller_->readThenWriteVelocity( + MotorIndex::FRONT_LEFT, target_wheel_rpms[FRONT_LEFT_WHEEL_SPACE_INDEX]), + motor_controller_->readThenWriteVelocity( + MotorIndex::BACK_LEFT, target_wheel_rpms[BACK_LEFT_WHEEL_SPACE_INDEX]), + motor_controller_->readThenWriteVelocity( + MotorIndex::BACK_RIGHT, target_wheel_rpms[BACK_RIGHT_WHEEL_SPACE_INDEX]), + }; - CHECK(encoder_calibrated_[FRONT_LEFT_MOTOR_CHIP_SELECT] && - encoder_calibrated_[FRONT_RIGHT_MOTOR_CHIP_SELECT] && - encoder_calibrated_[BACK_LEFT_MOTOR_CHIP_SELECT] && - encoder_calibrated_[BACK_RIGHT_MOTOR_CHIP_SELECT]) - << "Running without encoder calibration can cause serious harm, exiting"; + const double dribbler_rpm = motor_controller_->readThenWriteVelocity( + MotorIndex::DRIBBLER, dribbler_target_rpm_); - // Get current wheel electical RPM (don't account for pole pairs). We will use these - // for robot status feedback We assume the motors have ramped to the expected RPM from - // the previous iteration. - double front_right_velocity = - static_cast(tmc4671ReadThenWriteValue( - FRONT_RIGHT_MOTOR_CHIP_SELECT, TMC4671_PID_VELOCITY_ACTUAL, - TMC4671_PID_VELOCITY_TARGET, front_right_target_rpm)) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; - double front_left_velocity = - static_cast(tmc4671ReadThenWriteValue( - FRONT_LEFT_MOTOR_CHIP_SELECT, TMC4671_PID_VELOCITY_ACTUAL, - TMC4671_PID_VELOCITY_TARGET, front_left_target_rpm)) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; - double back_right_velocity = - static_cast(tmc4671ReadThenWriteValue( - BACK_RIGHT_MOTOR_CHIP_SELECT, TMC4671_PID_VELOCITY_ACTUAL, - TMC4671_PID_VELOCITY_TARGET, back_right_target_rpm)) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; - double back_left_velocity = - static_cast(tmc4671ReadThenWriteValue( - BACK_LEFT_MOTOR_CHIP_SELECT, TMC4671_PID_VELOCITY_ACTUAL, - TMC4671_PID_VELOCITY_TARGET, back_left_target_rpm)) * - MECHANICAL_MPS_PER_ELECTRICAL_RPM; - double dribbler_rpm = static_cast( - tmc4671ReadThenWriteValue(DRIBBLER_MOTOR_CHIP_SELECT, TMC4671_PID_VELOCITY_ACTUAL, - TMC4671_PID_VELOCITY_TARGET, dribbler_ramp_rpm_)); + const WheelSpace_t current_wheel_velocities = + current_wheel_rpms.cast() * drive_motor_mps_per_rpm_; - // Construct a MotorStatus object with the current velocities and dribbler rpm TbotsProto::MotorStatus motor_status = - updateMotorStatus(front_left_velocity, front_right_velocity, back_left_velocity, - back_right_velocity, dribbler_rpm); - - // This order needs to match euclidean_to_four_wheel converters order - // We also want to work in the meters per second space rather than electrical RPMs - WheelSpace_t current_wheel_velocities = {front_right_velocity, front_left_velocity, - back_left_velocity, back_right_velocity}; + createMotorStatus(current_wheel_velocities, dribbler_rpm); - // Run-away protection if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_->setValue(GpioState::LOW); + motor_controller_->immediatelyDisable(); LOG(FATAL) << "Front right motor runaway"; } else if (std::abs(current_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[FRONT_LEFT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_->setValue(GpioState::LOW); + motor_controller_->immediatelyDisable(); LOG(FATAL) << "Front left motor runaway"; } else if (std::abs(current_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[BACK_LEFT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_->setValue(GpioState::LOW); + motor_controller_->immediatelyDisable(); LOG(FATAL) << "Back left motor runaway"; } else if (std::abs(current_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[BACK_RIGHT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_->setValue(GpioState::LOW); + motor_controller_->immediatelyDisable(); LOG(FATAL) << "Back right motor runaway"; } // Convert to Euclidean velocity_delta - EuclideanSpace_t current_euclidean_velocity = + const EuclideanSpace_t current_euclidean_velocity = euclidean_to_four_wheel_.getEuclideanVelocity(current_wheel_velocities); motor_status.mutable_local_velocity()->set_x_component_meters( @@ -515,685 +175,118 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor motor_status.mutable_angular_velocity()->set_radians_per_second( current_euclidean_velocity[2]); - WheelSpace_t target_wheel_velocities = WheelSpace_t::Zero(); - // Get target wheel velocities from the primitive - if (motor.has_direct_per_wheel_control()) + if (motor_control.has_direct_per_wheel_control()) { - TbotsProto::MotorControl_DirectPerWheelControl direct_per_wheel = - motor.direct_per_wheel_control(); - target_wheel_velocities = { + const auto& direct_per_wheel = motor_control.direct_per_wheel_control(); + + target_wheel_velocities_ = { direct_per_wheel.front_right_wheel_velocity(), direct_per_wheel.front_left_wheel_velocity(), direct_per_wheel.back_left_wheel_velocity(), direct_per_wheel.back_right_wheel_velocity(), }; } - else if (motor.has_direct_velocity_control()) + else if (motor_control.has_direct_velocity_control()) { - TbotsProto::MotorControl_DirectVelocityControl direct_velocity = - motor.direct_velocity_control(); - EuclideanSpace_t target_euclidean_velocity = { + const auto& direct_velocity = motor_control.direct_velocity_control(); + + const EuclideanSpace_t target_euclidean_velocity = { -direct_velocity.velocity().y_component_meters(), direct_velocity.velocity().x_component_meters(), direct_velocity.angular_velocity().radians_per_second()}; - target_wheel_velocities = + motor_controller_->updateEuclideanVelocity(target_euclidean_velocity); + + target_wheel_velocities_ = euclidean_to_four_wheel_.getWheelVelocity(target_euclidean_velocity); } - // ramp the target velocities to keep acceleration compared to current velocities + // Ramp the target velocities to keep acceleration compared to current velocities // within safe bounds - target_wheel_velocities = euclidean_to_four_wheel_.rampWheelVelocity( - prev_wheel_velocities_, target_wheel_velocities, time_elapsed_since_last_poll_s); + target_wheel_velocities_ = euclidean_to_four_wheel_.rampWheelVelocity( + prev_wheel_velocities_, target_wheel_velocities_, time_elapsed_since_last_poll_s); + + const EuclideanSpace_t target_euclidean_velocity = + euclidean_to_four_wheel_.getEuclideanVelocity(target_wheel_velocities_); - prev_wheel_velocities_ = target_wheel_velocities; + motor_status.mutable_target_local_velocity()->set_x_component_meters( + target_euclidean_velocity[1]); + motor_status.mutable_target_local_velocity()->set_y_component_meters( + -target_euclidean_velocity[0]); + motor_status.mutable_target_angular_velocity()->set_radians_per_second( + target_euclidean_velocity[2]); - // Calculate speeds accounting for acceleration - front_right_target_rpm = - static_cast(target_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); - front_left_target_rpm = - static_cast(target_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); - back_left_target_rpm = - static_cast(target_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); - back_right_target_rpm = - static_cast(target_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] * - ELECTRICAL_RPM_PER_MECHANICAL_MPS); + prev_wheel_velocities_ = target_wheel_velocities_; // Get target dribbler rpm from the primitive int target_dribbler_rpm; - if (motor.drive_control_case() == + if (motor_control.drive_control_case() == TbotsProto::MotorControl::DriveControlCase::DRIVE_CONTROL_NOT_SET) { target_dribbler_rpm = 0; } else { - target_dribbler_rpm = motor.dribbler_speed_rpm(); + target_dribbler_rpm = motor_control.dribbler_speed_rpm(); } // Ramp the dribbler velocity // Clamp the max acceleration int max_dribbler_delta_rpm = static_cast( DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 * time_elapsed_since_last_poll_s); - int delta_rpm = std::clamp(target_dribbler_rpm - dribbler_ramp_rpm_, + int delta_rpm = std::clamp(target_dribbler_rpm - dribbler_target_rpm_, -max_dribbler_delta_rpm, max_dribbler_delta_rpm); - dribbler_ramp_rpm_ += delta_rpm; + dribbler_target_rpm_ += delta_rpm; // Clamp to the max rpm - int max_dribbler_rpm = - std::abs(static_cast(robot_constants_.max_force_dribbler_speed_rpm)); - dribbler_ramp_rpm_ = - std::clamp(dribbler_ramp_rpm_, -max_dribbler_rpm, max_dribbler_rpm); + int max_dribbler_rpm = std::abs(robot_constants_.max_force_dribbler_speed_rpm); + dribbler_target_rpm_ = + std::clamp(dribbler_target_rpm_, -max_dribbler_rpm, max_dribbler_rpm); - motor_status.mutable_dribbler()->set_dribbler_rpm(float(dribbler_ramp_rpm_)); + motor_status.mutable_dribbler()->set_dribbler_rpm( + static_cast(dribbler_target_rpm_)); return motor_status; } -bool MotorService::requiresMotorReinit(uint8_t motor) -{ - auto reset_search = - cached_motor_faults_[motor].motor_faults.find(TbotsProto::MotorFault::RESET); - - return !cached_motor_faults_[motor].drive_enabled || - (reset_search != cached_motor_faults_[motor].motor_faults.end()); -} - -void MotorService::spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, - uint32_t spi_speed) -{ - int ret; - - struct spi_ioc_transfer tr[1]; - memset(tr, 0, sizeof(tr)); - - tr[0].tx_buf = (unsigned long)tx_; - tr[0].rx_buf = (unsigned long)rx_; - tr[0].len = len; - tr[0].delay_usecs = 0; - tr[0].speed_hz = spi_speed; - tr[0].bits_per_word = 8; - - ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); - - CHECK(ret >= 1) << "SPI Transfer to motor failed, not safe to proceed: errno " - << strerror(errno); -} - - -void MotorService::readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, - const uint8_t* write_tx, - const uint8_t* read_rx, uint32_t spi_speed) -{ - uint8_t write_rx[5] = {0}; - - struct spi_ioc_transfer tr[2]; - memset(tr, 0, sizeof(tr)); - - tr[0].tx_buf = (unsigned long)read_tx; - tr[0].rx_buf = (unsigned long)read_rx; - tr[0].len = TMC_CMD_MSG_SIZE; - tr[0].delay_usecs = 0; - tr[0].speed_hz = spi_speed; - tr[0].bits_per_word = 8; - tr[0].cs_change = 0; - tr[1].tx_buf = (unsigned long)write_tx; - tr[1].rx_buf = (unsigned long)write_rx; - tr[1].len = TMC_CMD_MSG_SIZE; - tr[1].delay_usecs = 0; - tr[1].speed_hz = spi_speed; - tr[1].bits_per_word = 8; - tr[1].cs_change = 0; - - int ret1 = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); - int ret2 = ioctl(fd, SPI_IOC_MESSAGE(1), &tr[1]); - - CHECK(ret1 >= 1 && ret2 >= 1) - << "SPI Transfer to motor failed, not safe to proceed: errno " << strerror(errno); -} - - -// Both the TMC4671 (the controller) and the TMC6100 (the driver) respect -// the same SPI interface. So when we bind the API, we can use the same -// readWriteByte function, provided that the chip select pin is turning on -// the right chip. -// -// Each TMC4671 controller, TMC6100 driver and encoder group have their chip -// selects coming in from a demux (see diagram below). The demux is controlled -// by two bits {spi_demux_select_0, spi_demux_select_1}. If the bits are -// 10 the TMC4671 is selected, when the select bits are 01 the TMC6100 is -// selected and when they are 11 the encoder is selected. 00 disconnects all -// 3 chips. -// -// -// FRONT LEFT MOTOR -// CONTROLLER + DRIVER + ENCODER -// -// ┌───────┐ ┌───────────────┐ -// │ │ │ │ -// │ 2:4 │ 10 │ ┌─────────┐ │ -// │ ├────────┼──►TMC4671 │ │ B0 -// FRONT_LEFT_CS │ DEMUX │ │ └─────────┘ │ -// ───────────────► │ │ │ -// │ │ 01 │ ┌─────────┐ │ -// │ ├────────┼──►TMC6100 │ │ B1 -// │ │ │ └─────────┘ │ -// │ │ │ │ -// │ │ 11 │ ┌─────────┐ │ -// │ ├────────┼──►ENCODER │ │ B2 -// │ │ │ └─────────┘ │ -// └───▲───┘ │ │ -// │ └───────────────┘ -// │ -// spi_demux_sel_0 & 1 -// -uint8_t MotorService::tmc4671ReadWriteByte(uint8_t motor, uint8_t data, - uint8_t last_transfer) -{ - spi_demux_select_0_->setValue(GpioState::HIGH); - spi_demux_select_1_->setValue(GpioState::LOW); - return readWriteByte(motor, data, last_transfer, TMC4671_SPI_SPEED); -} - -int32_t MotorService::tmc4671ReadThenWriteValue(uint8_t motor, uint8_t read_addr, - uint8_t write_addr, int32_t write_data) -{ - spi_demux_select_0_->setValue(GpioState::HIGH); - spi_demux_select_1_->setValue(GpioState::LOW); - // ensure tx_ and rx_ are cleared - memset(read_tx_, 0, 5); - memset(write_tx_, 0, 5); - memset(read_rx_, 0, 5); - - // Trinamic transactions looks like this: - // + - - - + - - - + - - - + - - - + - - - + - // | ADDR | DATA | - // + - - - + - - - + - - - + - - - + - - - + - // 0 1 2 3 4 - // Also it is in BIG Endian, therefore MSB is leftmost bit of 0. - // For a write, MSB must be 1, for read, MSB must be 0 - // https://github.com/trinamic/TMC-API/blob/master/tmc/ic/TMC4671/TMC4671.c - read_tx_[0] = read_addr & 0x7f; - write_tx_[0] = write_addr | 0x80; - - // Convert from little endian to big endian - for (int i = 3; i >= 0; i--) - { - uint8_t byte_to_copy = (uint8_t)(0xff & (write_data >> 8 * i)); - write_tx_[4 - i] = byte_to_copy; - } - - readThenWriteSpiTransfer(file_descriptors_[motor], read_tx_, write_tx_, read_rx_, - TMC4671_SPI_SPEED); - - int32_t value = read_rx_[0]; - for (int i = 1; i < 5; i++) - { - value <<= 8; - value |= read_rx_[i]; - } - return value; -} - -uint8_t MotorService::tmc6100ReadWriteByte(uint8_t motor, uint8_t data, - uint8_t last_transfer) -{ - spi_demux_select_0_->setValue(GpioState::LOW); - spi_demux_select_1_->setValue(GpioState::HIGH); - return readWriteByte(motor, data, last_transfer, TMC6100_SPI_SPEED); -} - -uint8_t MotorService::readWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer, - uint32_t spi_speed) -{ - uint8_t ret_byte = 0; - - if (!transfer_started_) - { - memset(tx_, 0, sizeof(tx_)); - memset(rx_, 0, sizeof(rx_)); - position_ = 0; - - if (data & TMC_WRITE_BIT) - { - // If the transfer started and its a write operation, - // set the appropriate flags. - currently_reading_ = false; - currently_writing_ = true; - } - else - { - // The first byte should contain the address on a read operation. - // Trigger a transfer (1 byte) and buffer the response (4 bytes) - tx_[position_] = data; - spiTransfer(file_descriptors_[motor], tx_, rx_, 5, spi_speed); - - currently_reading_ = true; - currently_writing_ = false; - } - - transfer_started_ = true; - } - - if (currently_writing_) - { - // Buffer the data to send out when last_transfer is true. - tx_[position_++] = data; - } - - if (currently_reading_) - { - // If we are reading, we just need to return the buffered data - // byte by byte. - ret_byte = rx_[position_++]; - } - - if (currently_writing_ && last_transfer) - { - // we have all the bytes for this transfer, lets trigger the transfer and - // reset state - spiTransfer(file_descriptors_[motor], tx_, rx_, 5, spi_speed); - transfer_started_ = false; - } - - if (currently_reading_ && last_transfer) - { - // when reading, if last transfer is true, we just need to reset state - transfer_started_ = false; - } - - return ret_byte; -} - -void MotorService::writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int32_t value) +void MotorService::trackMotorReset() { - int num_retires_left = NUM_RETRIES_SPI; - int read_value = 0; + const auto now = std::chrono::steady_clock::now(); - // The SPI lines have a lot of noise, and sometimes a transfer will fail - // randomly. So we retry a few times before giving up. - while (num_retires_left > 0) + if (num_tracked_motor_resets_ == 0) { - tmc6100_writeInt(motor, address, value); - read_value = tmc6100_readInt(motor, address); - if (read_value == value) - { - return; - } - LOG(DEBUG) << "SPI Transfer to Driver Failed, retrying..."; - num_retires_left--; - } - - // If we get here, we have failed to write to the driver. We reset - // the chip to clear any bad values we just wrote and crash so everything stops. - reset_gpio_->setValue(GpioState::LOW); - CHECK(read_value == value) << "Couldn't write " << value - << " to the TMC6100 at address " << address - << " at address " << static_cast(address) - << " on motor " << static_cast(motor) - << " received: " << read_value; -} - -void MotorService::writeIntToTMC4671(uint8_t motor, uint8_t address, int32_t value) -{ - tmc4671_writeInt(motor, address, value); -} - -int MotorService::readIntFromTMC4671(uint8_t motor, uint8_t address) -{ - return tmc4671_readInt(motor, address); -} - -void MotorService::writeToControllerOrDieTrying(uint8_t motor, uint8_t address, - int32_t value) -{ - int num_retires_left = NUM_RETRIES_SPI; - int read_value = 0; - - // The SPI lines have a lot of noise, and sometimes a transfer will fail - // randomly. So we retry a few times before giving up. - while (num_retires_left > 0) - { - tmc4671_writeInt(motor, address, value); - read_value = tmc4671_readInt(motor, address); - if (read_value == value) - { - return; - } - LOG(DEBUG) << "SPI Transfer to Controller Failed, retrying..."; - num_retires_left--; - } - - // If we get here, we have failed to write to the controller. We reset - // the chip to clear any bad values we just wrote and crash so everything stops. - reset_gpio_->setValue(GpioState::LOW); - CHECK(read_value == value) << "Couldn't write " << value - << " to the TMC4671 at address " << address - << " at address " << static_cast(address) - << " on motor " << static_cast(motor) - << " received: " << read_value; -} - -void MotorService::configurePWM(uint8_t motor) -{ - LOG(INFO) << "Configuring PWM for motor " << static_cast(motor); - // Please read the header file and the datasheet for more info - writeToControllerOrDieTrying(motor, TMC4671_PWM_POLARITIES, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_PWM_MAXCNT, 0x00000F9F); - writeToControllerOrDieTrying(motor, TMC4671_PWM_BBM_H_BBM_L, 0x00002828); - writeToControllerOrDieTrying(motor, TMC4671_PWM_SV_CHOP, 0x00000107); -} - -void MotorService::configureDrivePI(uint8_t motor) -{ - LOG(INFO) << "Configuring Drive PI for motor " << static_cast(motor); - // Please read the header file and the datasheet for more info - // These values were calibrated using the TMC-IDE - writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 67109376); - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 67109376); - writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_P_VELOCITY_I, 52428800); - - // Explicitly disable the position controller - writeToControllerOrDieTrying(motor, TMC4671_PID_POSITION_P_POSITION_I, 0); - - writeToControllerOrDieTrying(motor, TMC4671_PIDOUT_UQ_UD_LIMITS, 32767); - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 2500); - writeToControllerOrDieTrying(motor, TMC4671_PID_ACCELERATION_LIMIT, 1000); - - writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_LIMIT, 45000); - - tmc4671_switchToMotionMode(motor, TMC4671_MOTION_MODE_VELOCITY); -} - -void MotorService::configureDribblerPI(uint8_t motor) -{ - LOG(INFO) << "Configuring Dribbler PI for motor " << static_cast(motor); - // Please read the header file and the datasheet for more info - // These values were calibrated using the TMC-IDE - writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 39337600); - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 39333600); - writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_P_VELOCITY_I, 2621448); - - // Explicitly disable the position controller - writeToControllerOrDieTrying(motor, TMC4671_PID_POSITION_P_POSITION_I, 0); - - writeToControllerOrDieTrying(motor, TMC4671_PIDOUT_UQ_UD_LIMITS, 32767); - // TODO (#2677) support MAX_FORCE mode. This value can go up to 4.8 amps but we set it - // to 2 for now (sufficient for INDEFINITE mode). - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 4000); - writeToControllerOrDieTrying(motor, TMC4671_PID_ACCELERATION_LIMIT, 40000); - writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_LIMIT, 15000); -} - -void MotorService::configureADC(uint8_t motor) -{ - LOG(INFO) << "Configuring ADC for motor " << static_cast(motor); - // ADC configuration - writeToControllerOrDieTrying(motor, TMC4671_ADC_I_SELECT, 0x18000100); - writeToControllerOrDieTrying(motor, TMC4671_dsADC_MDEC_B_MDEC_A, 0x014E014E); - - // These values have been calibrated for the TI INA240 current sense amplifier. - // The scaling is also set to work with both the drive and dribbler motors. - // - // If you wish to use the TMC4671+TMC6100-BOB you can use the following values, - // that work for the AD8418 current sense amplifier - // - // TMC4671_ADC_I0_SCALE_OFFSET = 0x010081DD - // TMC4671_ADC_I1_SCALE_OFFSET = 0x0100818E - // - writeToControllerOrDieTrying(motor, TMC4671_ADC_I0_SCALE_OFFSET, 0x000981DD); - writeToControllerOrDieTrying(motor, TMC4671_ADC_I1_SCALE_OFFSET, 0x0009818E); -} - -void MotorService::configureEncoder(uint8_t motor) -{ - LOG(INFO) << "Configuring Encoder for motor " << static_cast(motor); - // ABN encoder settings - writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_MODE, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_PPR, 0x00001000); -} - -void MotorService::configureHall(uint8_t motor) -{ - LOG(INFO) << "Configuring Hall for motor " << static_cast(motor); - // Digital hall settings - writeToControllerOrDieTrying(motor, TMC4671_HALL_MODE, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_HALL_PHI_E_PHI_M_OFFSET, 0x00000000); - - // Feedback selection - writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, TMC4671_PHI_E_HALL); - writeToControllerOrDieTrying(motor, TMC4671_VELOCITY_SELECTION, - TMC4671_VELOCITY_PHI_E_HAL); -} - -void MotorService::startEncoderCalibration(uint8_t motor) -{ - LOG(WARNING) << "Calibrating the encoder, ensure the robot is lifted off the ground"; - - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 0x000003E8); - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 0x01000100); - writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 0x01000100); - - writeToControllerOrDieTrying(motor, TMC4671_MODE_RAMP_MODE_MOTION, 0x00000008); - writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_PHI_E_PHI_M_OFFSET, - 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, 0x00000001); - writeToControllerOrDieTrying(motor, TMC4671_PHI_E_EXT, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000FFF); -} - -void MotorService::endEncoderCalibration(uint8_t motor) -{ - LOG(WARNING) << "Calibrating the encoder, wheels may move"; - - writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_COUNT, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, TMC4671_PHI_E_ABN); - - encoder_calibrated_[motor] = true; - - configureDrivePI(motor); -} - -void MotorService::runOpenLoopCalibrationRoutine(uint8_t motor, size_t num_samples) -{ - // Some limits - tmc4671_writeInt(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 0x000003E8); - tmc4671_writeInt(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 0x01000100); - tmc4671_writeInt(motor, TMC4671_PID_FLUX_P_FLUX_I, 0x01000100); - - // Open loop settings - tmc4671_writeInt(motor, TMC4671_OPENLOOP_MODE, 0x00000000); - tmc4671_writeInt(motor, TMC4671_OPENLOOP_ACCELERATION, 0x0000003C); - tmc4671_writeInt(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0xFFFFFFFB); - - // Feedback selection - tmc4671_writeInt(motor, TMC4671_PHI_E_SELECTION, TMC4671_PHI_E_OPEN_LOOP); - tmc4671_writeInt(motor, TMC4671_UQ_UD_EXT, 0x00000799); - - // Switch to open loop velocity mode - tmc4671_writeInt(motor, TMC4671_MODE_RAMP_MODE_MOTION, 0x00000008); - - // Rotate right - tmc4671_writeInt(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x0000004A); - - // Setup CSVs - LOG(CSV, "encoder_calibration_" + std::to_string(motor) + ".csv") - << "actual_encoder,estimated_phi\n"; - LOG(CSV, "phase_currents_and_voltages_" + std::to_string(motor) + ".csv") - << "adc_iv,adc_ux,adc_wy,pwm_iv,pwm_ux,pwm_wy\n"; - - // Take samples of the useful registers - for (size_t num_sample = 0; num_sample < num_samples; num_sample++) - { - int estimated_phi = tmc4671_readInt(motor, TMC4671_OPENLOOP_PHI); - int actual_encoder = tmc4671_readRegister16BitValue( - motor, TMC4671_ABN_DECODER_PHI_E_PHI_M, BIT_16_TO_31); - - LOG(CSV, "encoder_calibration_" + std::to_string(motor) + ".csv") - << actual_encoder << "," << estimated_phi << "\n"; - - int16_t adc_v = - tmc4671_readRegister16BitValue(motor, TMC4671_ADC_IV, BIT_0_TO_15); - int16_t adc_u = - tmc4671_readRegister16BitValue(motor, TMC4671_ADC_IWY_IUX, BIT_0_TO_15); - int16_t adc_w = - tmc4671_readRegister16BitValue(motor, TMC4671_ADC_IWY_IUX, BIT_16_TO_31); - - tmc4671_writeInt(motor, TMC4671_INTERIM_ADDR, INTERIM_ADDR_PWM_UV); - int16_t pwm_v = - tmc4671_readRegister16BitValue(motor, TMC4671_INTERIM_DATA, BIT_0_TO_15); - - tmc4671_writeInt(motor, TMC4671_INTERIM_ADDR, INTERIM_ADDR_PWM_WY_UX); - int16_t pwm_u = - tmc4671_readRegister16BitValue(motor, TMC4671_INTERIM_DATA, BIT_0_TO_15); - int16_t pwm_w = - tmc4671_readRegister16BitValue(motor, TMC4671_INTERIM_DATA, BIT_16_TO_31); - - LOG(CSV, "phase_currents_and_voltages_" + std::to_string(motor) + ".csv") - << adc_v << "," << adc_u << "," << adc_w << "," << pwm_v << "," << pwm_u - << "," << pwm_w << "\n"; + tracked_motor_reset_start_time_ = now; + num_tracked_motor_resets_ = 1; + return; } - // Stop open loop rotation - tmc4671_writeInt(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x00000000); -} - -void MotorService::startDriver(uint8_t motor) -{ - // Set the drive strength to 0, the weakest it can go as recommended - // by the TMC4671-TMC6100-BOB datasheet. - int32_t current_drive_conf = tmc6100_readInt(motor, TMC6100_DRV_CONF); - writeToDriverOrDieTrying(motor, TMC6100_DRV_CONF, - current_drive_conf & (~TMC6100_DRVSTRENGTH_MASK)); - writeToDriverOrDieTrying(motor, TMC6100_GCONF, 0x40); - - // All default but updated SHORTFILTER to 2us to avoid false positive shorts - // detection. - writeToDriverOrDieTrying(motor, TMC6100_SHORT_CONF, 0x13020606); - - LOG(DEBUG) << "Driver " << std::to_string(motor) << " accepted conf"; -} - -void MotorService::startController(uint8_t motor, bool dribbler) -{ - // Read the chip ID to validate the SPI connection - tmc4671_writeInt(motor, TMC4671_CHIPINFO_ADDR, 0x000000000); - int chip_id = tmc4671_readInt(motor, TMC4671_CHIPINFO_DATA); - - CHECK(0x34363731 == chip_id) << "The TMC4671 of motor " - << static_cast(motor) << " is not responding"; - - LOG(DEBUG) << "Controller " << std::to_string(motor) - << " online, responded with: " << chip_id; - - // Configure common controller params - configurePWM(motor); - configureADC(motor); + const auto elapsed_s = std::chrono::duration_cast( + now - tracked_motor_reset_start_time_) + .count(); - if (dribbler) + if (elapsed_s < MOTOR_RESET_TIME_THRESHOLD_S) { - // Configure to brushless DC motor with 1 pole pair - writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, 0x00030001); - configureHall(motor); - - configureDribblerPI(motor); + num_tracked_motor_resets_++; } else { - // Configure to brushless DC motor with 8 pole pairs - writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, 0x00030008); - configureEncoder(motor); - } -} - -void MotorService::checkEncoderConnections() -{ - LOG(INFO) << "Starting encoder connection check!"; - - std::vector calibrated_motors(NUM_DRIVE_MOTORS, false); - std::vector initial_velocities(NUM_DRIVE_MOTORS, 0); - - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; ++motor) - { - // read back current velocity - initial_velocities[motor] = tmc4671_readInt(motor, TMC4671_ABN_DECODER_COUNT); - - // open loop mode can be used without an encoder, set open loop phi positive - // direction - writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_MODE, 0x00000000); - writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, - TMC4671_PHI_E_OPEN_LOOP); - writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_ACCELERATION, 0x0000003C); - - // represents effective voltage applied to the motors (% voltage) - writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000799); - - // uq_ud_ext mode - writeToControllerOrDieTrying(motor, TMC4671_MODE_RAMP_MODE_MOTION, 0x00000008); - - // 10 RPM - writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x0000000A); - } - - for (int num_iterations = 0; - num_iterations < 10 && - std::any_of(calibrated_motors.begin(), calibrated_motors.end(), - [](bool calibration_status) { return !calibration_status; }); - ++num_iterations) - { - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; ++motor) - { - if (calibrated_motors[motor]) - { - continue; - } - // now read back the velocity - int read_back_velocity = tmc4671_readInt(motor, TMC4671_ABN_DECODER_COUNT); - LOG(INFO) << MOTOR_NAMES[motor] << " read back: " << read_back_velocity - << " and initially read: " << initial_velocities[motor]; - - if (read_back_velocity != initial_velocities[motor]) - { - calibrated_motors[motor] = true; - } - } - - // sleep for 100 milliseconds - usleep(MICROSECONDS_PER_MILLISECOND * 100); - } - - bool calibrated = true; - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; ++motor) - { - if (!calibrated_motors[motor]) - { - calibrated = false; - LOG(WARNING) << "Encoder calibration check failure. " << MOTOR_NAMES[motor] - << " did not change as expected"; - } - } - if (!calibrated) - { - LOG(FATAL) - << "Encoder calibration check failure. Not all encoders responded as expected"; + tracked_motor_reset_start_time_ = now; + num_tracked_motor_resets_ = 1; } - // stop all motors, reset back to velocity control mode - for (uint8_t motor = 0; motor < NUM_DRIVE_MOTORS; ++motor) + if (num_tracked_motor_resets_ > MOTOR_RESET_THRESHOLD_COUNT) { - writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x00000000); - tmc4671_switchToMotionMode(motor, TMC4671_MOTION_MODE_VELOCITY); + LOG(FATAL) << "Motors reset too frequently (" << num_tracked_motor_resets_ + << " times in " << elapsed_s + << " seconds). Thunderloop crashing for safety."; } - - LOG(INFO) << "All encoders appear to be connected!"; } -void MotorService::resetMotorBoard() +bool MotorService::anyMotorRequiresReset() const { - reset_gpio_->setValue(GpioState::LOW); -} + return std::any_of(reflective_enum::values().begin(), + reflective_enum::values().end(), + [&](const MotorIndex motor) + { return motor_controller_->checkFaults(motor).requiresReset(); }); +} \ No newline at end of file diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 3efbb40799..c05355e695 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -1,22 +1,16 @@ #pragma once -#include #include -#include -#include #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" -#include "shared/constants.h" #include "shared/robot_constants.h" -#include "software/embedded/constants/constants.h" -#include "software/embedded/gpio.h" -#include "software/embedded/gpio_char_dev.h" -#include "software/embedded/gpio_sysfs.h" +#include "software/embedded/gpio/gpio.h" +#include "software/embedded/motor_controller/motor_controller.h" #include "software/physics/euclidean_to_wheel.h" /** - * A service that interacts with the motor. + * A service that interacts with the motors. * * It is responsible for: * - Converting Euclidean velocities to wheel velocities @@ -26,417 +20,88 @@ class MotorService { public: - // SPI Chip Selects - static const uint8_t FRONT_LEFT_MOTOR_CHIP_SELECT = 0; - static const uint8_t FRONT_RIGHT_MOTOR_CHIP_SELECT = 3; - static const uint8_t BACK_LEFT_MOTOR_CHIP_SELECT = 1; - static const uint8_t BACK_RIGHT_MOTOR_CHIP_SELECT = 2; - /** - * Service that interacts with the motor board. - * Opens all the required ports and maintains them until destroyed. + * Service that interacts with the motors. * * @param robot_constants The robot constants - * @param control_loop_frequency_hz The frequency the main loop will call poll at */ - MotorService(const robot_constants::RobotConstants& robot_constants, - int control_loop_frequency_hz); - - virtual ~MotorService(); + explicit MotorService(const robot_constants::RobotConstants& robot_constants); /** - * When the motor service is polled with a DirectControlPrimitive msg, - * call the appropriate trinamic api function to spin the appropriate motor. + * Polls the motor service to execute the given motor control command and + * return the current motor status. * - * @param motor The motor msg to unpack and execute on the motors - * @param time_elapsed_since_last_poll_s The time since last poll was called in - * seconds - * @returns MotorStatus The status of all the drive units + * @param motor_control The motor control message to unpack and execute + * @param time_elapsed_since_last_poll_s The time since the last poll in seconds + * @return The status of all the motors */ TbotsProto::MotorStatus poll(const TbotsProto::MotorControl& motor_control, double time_elapsed_since_last_poll_s); - /** - * Trinamic API binding, sets spi_demux_select_0|1 pins - * appropriately and calls readWriteByte. See C++ implementation file for more info - * - * @param motor Which motor to talk to (in our case, the chip select) - * @param data The data to send - * @param last_transfer The last transfer of uint8_t data for this transaction. - * @return A byte read from the trinamic chip - */ - uint8_t tmc4671ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer); - uint8_t tmc6100ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer); - - /* - * For FOC to work, the controller needs to know the electical angle of the rotor - * relative to the mechanical angle of the rotor. In an incremental-encoder-only - * setup, we can energize the motor coils so that the rotor locks itself along - * one of its pole-pairs, allowing us to reset the encoder. - * - * WARNING: Do not try to spin the motor without initializing the encoder! - * The motor can overheat if the TMC4671 doesn't auto shut-off. - * - * There are some safety checks to ensure that the encoder is - * initialized, do not tamper with them. You have been warned. - * - * @param motor The motor to initialize the encoder for - */ - void startEncoderCalibration(uint8_t motor); - void endEncoderCalibration(uint8_t motor); - - /** - * Spin the motor in openloop mode (safe to run before encoder initialization) - * - * Captures TMC4671_OPENLOOP_PHI and TMC4671_ABN_DECODER_PHI_E and stores it - * in encoder_calibration.csv - * - * Captures adc_iv, adc_ux, adc_wy and pwm_iv, pwm_ux, pwm_wy. - * - * WARNING: Make sure adc_iv is in phase with pwm_iv, adc_ux is in phase - * with pwm_ux, and adc_wy is in phase with pwm_wy. - * - * If you _dont_ do this, you can risk burning the motor. - * - * @param motor The motor to spin in openloop mode - * @param num_samples The number of samples to take on each - */ - void runOpenLoopCalibrationRoutine(uint8_t motor, size_t num_samples); - - /** - * Reset the motor board by toggling the reset GPIO appropriately. Effectively stops - * the motors from moving. - */ - void resetMotorBoard(); - /** * Clears previous faults, configures the motor and checks encoder connections. */ void setup(); /** - * Holds motor fault information for a particular motor and whether any fault has - * caused the motor to be disabled. - */ - struct MotorFaultIndicator - { - bool drive_enabled; - std::unordered_set motor_faults; - - /** - * Construct a default indicator of no faults and running motors. - */ - MotorFaultIndicator() : drive_enabled(true), motor_faults() {} - - /** - * Construct an indicator with faults and whether the motor is enabled. - * - * @param drive_enabled true if the motor is enabled, false if disabled due to a - * motor fault - * @param motor_faults a set of faults associated with this motor - */ - MotorFaultIndicator(bool drive_enabled, - std::unordered_set& motor_faults) - : drive_enabled(drive_enabled), motor_faults(motor_faults) - { - } - }; - - /** - * Log the driver fault in a human readable log msg - * - * @param motor The motor to log the status for - * - * @return a struct containing the motor faults and whether the motor was disabled due - * to the fault + * Resets the motors, effectively stopping them from moving. */ - struct MotorFaultIndicator checkDriverFault(uint8_t motor); - - /** - * Sets up motor as drive motor controllers - * - * @param motor drive motor number - */ - void setUpDriveMotor(uint8_t motor); - - /** - * Used for testing purposes: - * - * Wrapper function that writes int to the TMC4671 - * @param motor drive motor number - * @param address motor address - * @param value write value - */ - void writeIntToTMC4671(uint8_t motor, uint8_t address, int32_t value); - - /** - * Used for testing purposes: - * - * Wrapper function that reads int from the TMC4671 - * @param motor drive motor number - * @param address motor address - * @return read value - */ - int readIntFromTMC4671(uint8_t motor, uint8_t address); + void reset(); private: /** - * Initializes Motor Service - * - * @param robot_constants robot constants for motor service - * @param control_loop_frequency_hz control loop frequency in Hertz - */ - void motorServiceInit(const robot_constants::RobotConstants& robot_constants, - int control_loop_frequency_hz); - - /** - * Calls the configuration functions below in the right sequence - * - * @param motor The motor setup the driver/controller for - * @param dribbler If true, configures the motor to be a dribbler - */ - void startDriver(uint8_t motor); - void startController(uint8_t motor, bool dribbler); - - /** - * Configuration settings - * - * These values were determined by reading the datasheets and user manual - * here: https://www.trinamic.com/support/eval-kits/details/tmc4671-tmc6100-bob/ - * - * If you are planning to change these settings, I highly recommend that you - * plug the motor + encoder pair in the TMC-IDE and use the TMC4671 EVAL - * with the TMC6100 EVAL to get the motor spinning. - * - * Then using the exported registers as a baseline, you can use the - * runOpenLoopCalibrationRoutine and plot the generated csvs. These csvs capture the - * data for encoder calibration and adc configuration, the two most important steps - * for the motor to work. Page 143 (title Setup Guidelines) of the TMC4671 is very - * useful. - * - * @param motor The motor to configure (the same value as the chip select) - */ - void configurePWM(uint8_t motor); - void configureDribblerPI(uint8_t motor); - void configureDrivePI(uint8_t motor); - void configureADC(uint8_t motor); - void configureEncoder(uint8_t motor); - void configureHall(uint8_t motor); - - /** - * A lot of initialization parameters are necessary to function. Even if - * there is a single bit error, we can risk frying the motor driver or - * controller. - * - * The following functions can be used to setup initialization params - * that _must_ be set to continue. A failed call will crash the program - * - * @param motor Which motor to talk to (in our case, the chip select) - * @param address The address to send data to - * @param value The value to write - * - */ - void writeToControllerOrDieTrying(uint8_t motor, uint8_t address, int32_t value); - void writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int32_t value); - - /** - * Trigger an SPI transfer over an open SPI connection - * - * @param fd The SPI File Descriptor to transfer data over - * @param tx The tx buffer, data to send out - * @param rx The rx buffer, will be updated with data from the full-duplex transfer - * @param len The length of the tx and rx buffer - * @param spi_speed The speed to run spi at - * - */ - void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, - uint32_t spi_speed); - - /** - * Performs two back to back SPI transactions, first a read and then a write. - * NOTE: read_tx and write_tx must both be in BIG ENDIAN, as required by the - * Trinamic controller - * - * @param fd the SPI file descriptor to transfer data over - * @param read_tx pointer to the buffer containing the address for reading - * @param write_tx pointer to the buffer containing the address + data for write - * @param read_rx the buffer our read response will be placed in - * @param spi_speed the speed to run spi at - */ - void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, uint8_t const* write_tx, - uint8_t const* read_rx, uint32_t spi_speed); - - /** - * A function which is written in the same style as the rest of the Trinamic API. - * This will trigger two SPI transactions back to back, reading a value and then - * writing a value for a specific motor - * - * @param motor The motor we want to read & write from - * @param read_addr the address of the register to read - * @param write_addr the address of the register to write - * @param write_data the data to write - * @return the value read from the trinamic controller - */ - int32_t tmc4671ReadThenWriteValue(uint8_t motor, uint8_t read_addr, - uint8_t write_addr, int32_t write_data); - - /** - * Trinamic API Binding function + * Creates a motor controller based on the motor board type specified at compile time. * - * @param motor Which motor to talk to (in our case, the chip select) - * @param data The data to send - * @param last_transfer The last transfer of uint8_t data for this transaction. - * @param spi_speed The speed to run spi at - * - * @return A byte read from the trinamic chip + * @return the instantiated motor controller implementation corresponding to the + * motor board type specified at compile time */ - uint8_t readWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer, - uint32_t spi_speed); - + std::unique_ptr setupMotorController(); /** - * Spin each motor of the NUM_DRIVE_MOTORS in open loop mode to check if the encoder - * is responding as expected. Allows us to do a basic test of whether the encoder is - * physically connected to the motor board. + * Return a MotorStatus proto filled with motor velocities and faults. * - * Leaves the motors connected in MOTION_MODE_VELOCITY - */ - void checkEncoderConnections(); - - /** - * Return a MotorStatus proto filled with motor faults. Some values of the proto are - * previously cached velocities due to SPI transaction costs. - * - * @param front_left_wheel_velocity_mps the front left motor's velocity in - * mechanical MPS - * @param front_right_velocity_mps the front right motor's velocity in - * mechanical MPS - * @param back_left velocity_mps the back left motor's velocity in - * mechanical MPS - * @param back_right_velocity_mps the back right motor's velocity in - * mechanical MPS - * @param dribbler_rpm the dribbler's rotations per minute + * @param current_wheel_velocities the current wheel velocities in m/s + * @param dribbler_rpm the dribbler motor's rotations per minute * * @return a MotorStatus proto with the velocity of each motor as well as their fault * statuses (some faults may be cached) */ - TbotsProto::MotorStatus updateMotorStatus(double front_left_velocity_mps, - double front_right_velocity_mps, - double back_left_velocity_mps, - double back_right_velocity_mps, - double dribbler_rpm); + TbotsProto::MotorStatus createMotorStatus( + const WheelSpace_t& current_wheel_velocities, double dribbler_rpm) const; /** - * Helper function to setup a GPIO pin. Selects the appropriate GPIO implementation - * based on the host platform. - * - * @tparam T The representation of the GPIO number - * @param gpio_number The GPIO number (this is typically different from the hardware - * pin number) - * @param direction The direction of the GPIO pin (input or output) - * @param initial_state The initial state of the GPIO pin (high or low) + * Tracks that a motor reset occurred just now. + * We count the number of resets within a certain time threshold, and if + * this count exceeds a certain level, we crash Thunderloop for safety. */ - template - static std::unique_ptr setupGpio(const T& gpio_number, GpioDirection direction, - GpioState initial_state); + void trackMotorReset(); /** - * Returns true if we've detected a RESET in our cached motor faults indicators or if - * we have a fault that disables drive. + * Checks if any motor requires a reset due to being disabled or having a + * reset fault. * - * @param motor chip select to check for RESETs - * - * @return true if the motor has returned a cached RESET fault, false otherwise + * @return true if any motor requires a reset, false otherwise */ - bool requiresMotorReinit(uint8_t motor); - - // All trinamic RPMS are electrical RPMS, they don't factor in the number of pole - // pairs of the drive motor. - // - // TODO (#2720): compute from robot constants (this was computed by hand and is - // accurate) - static constexpr double MECHANICAL_MPS_PER_ELECTRICAL_RPM = 0.000111; - static constexpr double ELECTRICAL_RPM_PER_MECHANICAL_MPS = - 1 / MECHANICAL_MPS_PER_ELECTRICAL_RPM; - - // to check if the motors have been calibrated - bool is_initialized_ = false; - - // Select between driver and controller gpio - std::unique_ptr spi_demux_select_0_; - std::unique_ptr spi_demux_select_1_; - - // Enable driver gpio - std::unique_ptr driver_control_enable_gpio_; - std::unique_ptr reset_gpio_; - - // Transfer Buffers for spiTransfer - uint8_t tx_[5] = {0}; - uint8_t rx_[5] = {0}; - - // Transfer Buffers for readThenWriteSpiTransfer - uint8_t write_tx_[5] = {0}; - uint8_t read_tx_[5] = {0}; - uint8_t read_rx_[5] = {0}; - - // Transfer State - bool transfer_started_ = false; - bool currently_writing_ = false; - bool currently_reading_ = false; - uint8_t position_ = 0; - - // SPI File Descriptors - std::unordered_map file_descriptors_; + bool anyMotorRequiresReset() const; robot_constants::RobotConstants robot_constants_; - // Drive Motors + std::unique_ptr motor_controller_; + EuclideanToWheel euclidean_to_four_wheel_; - std::unordered_map encoder_calibrated_; - std::unordered_map cached_motor_faults_; - // Previous wheel velocities WheelSpace_t prev_wheel_velocities_; + WheelSpace_t target_wheel_velocities_; - int front_left_target_rpm = 0; - int front_right_target_rpm = 0; - int back_left_target_rpm = 0; - int back_right_target_rpm = 0; - - // the motor cs id to check for motor faults - uint8_t motor_fault_detector_; - - static const int NUM_CALIBRATION_ATTEMPTS = 10; + int dribbler_target_rpm_; - int dribbler_ramp_rpm_; + double drive_motor_mps_per_rpm_; - std::optional> - tracked_motor_fault_start_time_; + std::chrono::time_point tracked_motor_reset_start_time_; int num_tracked_motor_resets_; - static const uint8_t DRIBBLER_MOTOR_CHIP_SELECT = 4; - - static const uint8_t NUM_DRIVE_MOTORS = 4; - static const uint8_t NUM_MOTORS = NUM_DRIVE_MOTORS + 1; - - static const int MOTOR_FAULT_TIME_THRESHOLD_S = 60; - static const int MOTOR_FAULT_THRESHOLD_COUNT = 3; - - // SPI Trinamic Motor Driver Paths (indexed with chip select above) - static constexpr const char* SPI_PATHS[] = {"/dev/spidev0.0", "/dev/spidev0.1", - "/dev/spidev0.2", "/dev/spidev0.3", - "/dev/spidev0.4"}; - - // Motor names (indexed with chip select above) - static constexpr const char* MOTOR_NAMES[] = {"front_left", "back_left", "back_right", - "front_right", "dribbler"}; -}; - -template -std::unique_ptr MotorService::setupGpio(const T& gpio_number, - GpioDirection direction, - GpioState initial_state) -{ - return std::make_unique(gpio_number, direction, initial_state, - "/dev/gpiochip4"); -} + static constexpr int MOTOR_RESET_TIME_THRESHOLD_S = 60; + static constexpr int MOTOR_RESET_THRESHOLD_COUNT = 3; + static constexpr double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; + static constexpr int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; +}; \ No newline at end of file From db6cb7fe11f5f8f290054823eae749e65b625f50 Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 16:51:38 -0700 Subject: [PATCH 06/23] remove gpio libraries --- src/software/embedded/gpio_sysfs.cpp | 88 ---------------------------- src/software/embedded/gpio_sysfs.h | 47 --------------- 2 files changed, 135 deletions(-) delete mode 100644 src/software/embedded/gpio_sysfs.cpp delete mode 100644 src/software/embedded/gpio_sysfs.h diff --git a/src/software/embedded/gpio_sysfs.cpp b/src/software/embedded/gpio_sysfs.cpp deleted file mode 100644 index 233f11d912..0000000000 --- a/src/software/embedded/gpio_sysfs.cpp +++ /dev/null @@ -1,88 +0,0 @@ -#include "software/embedded/gpio_sysfs.h" - -#include -#include -#include - -#include -#include -#include - -#include "software/logger/logger.h" - -GpioSysfs::GpioSysfs(const std::string& gpio_number, GpioDirection direction, - GpioState initial_state) -{ - // Setup the provided GPIO pin - gpio_number_ = gpio_number; - - auto export_gpio_fs = std::ofstream("/sys/class/gpio/export"); - export_gpio_fs << gpio_number; - export_gpio_fs.close(); - - auto set_direction_fs = - std::ofstream("/sys/class/gpio/gpio" + gpio_number + "/direction"); - - switch (direction) - { - case GpioDirection::OUTPUT: - { - set_direction_fs << "out"; - break; - } - case GpioDirection::INPUT: - { - set_direction_fs << "in"; - break; - } - } - - set_direction_fs.close(); - setValue(initial_state); - - LOG(DEBUG) << "GPIO " << gpio_number_ << " online"; -} - -void GpioSysfs::setValue(GpioState state) -{ - std::ofstream gpio_fs("/sys/class/gpio/gpio" + gpio_number_ + "/value"); - - CHECK(gpio_fs.is_open()) << "Could not set GPIO pin"; - - switch (state) - { - case GpioState::HIGH: - { - gpio_fs << "1"; - break; - } - case GpioState::LOW: - { - gpio_fs << "0"; - break; - } - } - - if (gpio_fs.is_open()) - { - gpio_fs.close(); - } -} - -GpioState GpioSysfs::getValue() -{ - std::ifstream gpio_fs("/sys/class/gpio/gpio" + gpio_number_ + "/value"); - std::string level; - - CHECK(gpio_fs.is_open()) << "Could not read GPIO pin"; - std::getline(gpio_fs, level); - - if (level.compare("0") == 0) - { - return GpioState::LOW; - } - else - { - return GpioState::HIGH; - } -} diff --git a/src/software/embedded/gpio_sysfs.h b/src/software/embedded/gpio_sysfs.h deleted file mode 100644 index 8d8501cd04..0000000000 --- a/src/software/embedded/gpio_sysfs.h +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include -#include - -#include "software/embedded/gpio.h" - -/** - * GPIO with the sysfs interface - * - * Deprecated in Linux kernels 4.8 and later but still available on some systems. - */ -class GpioSysfs : public Gpio -{ - public: - /* - * GPIO Sysfs Wrapper - * - * See https://www.kernel.org/doc/Documentation/gpio/sysfs.txt - * - * @param gpio The gpio to setup - * @param direction The direction to configure this gpio in - * @param initial_state The initial GpioState of the pin - */ - GpioSysfs(const std::string& gpio_number, GpioDirection direction, - GpioState initial_state); - - /** - * Set the value to the provided state - * - * @param state The state - */ - void setValue(GpioState state) override; - - /** - * Get the current state of the gpio - */ - GpioState getValue() override; - - private: - std::string gpio_number_; -}; From 17769aa9ac8dd31da1ebdbfaec3e1dc4ae48f000 Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 16:55:12 -0700 Subject: [PATCH 07/23] spi utils --- src/software/embedded/spi_utils.cpp | 59 +++++++++++++++++++++++++++++ src/software/embedded/spi_utils.h | 36 ++++++++++++++++++ 2 files changed, 95 insertions(+) create mode 100644 src/software/embedded/spi_utils.cpp create mode 100644 src/software/embedded/spi_utils.h diff --git a/src/software/embedded/spi_utils.cpp b/src/software/embedded/spi_utils.cpp new file mode 100644 index 0000000000..0962d6e4ee --- /dev/null +++ b/src/software/embedded/spi_utils.cpp @@ -0,0 +1,59 @@ +#include "software/embedded/spi_utils.h" + +#include +#include +#include + +#include "software/logger/logger.h" + +void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, + uint32_t spi_speed) +{ + int ret; + + struct spi_ioc_transfer tr[1]; + memset(tr, 0, sizeof(tr)); + + tr[0].tx_buf = (unsigned long)tx; + tr[0].rx_buf = (unsigned long)rx; + tr[0].len = len; + tr[0].delay_usecs = 0; + tr[0].speed_hz = spi_speed; + tr[0].bits_per_word = 8; + + ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); + + CHECK(ret >= 1) << "SPI Transfer to motor failed, not safe to proceed: errno " + << strerror(errno); +} + +void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, const uint8_t* write_tx, + const uint8_t* read_rx, const uint32_t read_len, + const uint32_t write_len, uint32_t spi_speed) +{ + uint8_t write_rx[5] = {0}; + + struct spi_ioc_transfer tr[2]; + memset(tr, 0, sizeof(tr)); + + tr[0].tx_buf = (unsigned long)read_tx; + tr[0].rx_buf = (unsigned long)read_rx; + tr[0].len = read_len; + tr[0].delay_usecs = 0; + tr[0].speed_hz = spi_speed; + tr[0].bits_per_word = 8; + tr[0].cs_change = 0; + tr[1].tx_buf = (unsigned long)write_tx; + tr[1].rx_buf = (unsigned long)write_rx; + tr[1].len = write_len; + tr[1].delay_usecs = 0; + tr[1].speed_hz = spi_speed; + tr[1].bits_per_word = 8; + tr[1].cs_change = 0; + + int ret1 = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); + int ret2 = ioctl(fd, SPI_IOC_MESSAGE(1), &tr[1]); + + CHECK(ret1 >= 1 && ret2 >= 1) + << "SPI Transfer to motor failed, not safe to proceed: errno " << strerror(errno); +} \ No newline at end of file diff --git a/src/software/embedded/spi_utils.h b/src/software/embedded/spi_utils.h new file mode 100644 index 0000000000..30aaea4f67 --- /dev/null +++ b/src/software/embedded/spi_utils.h @@ -0,0 +1,36 @@ +#pragma once + +#include + +#include + +// Wrap in spi_utils namespace +/** + * Trigger an SPI transfer over an open SPI connection + * + * NOTE: tx is expected to be in BIG ENDIAN + * + * @param fd the SPI file descriptor to transfer data over + * @param tx the tx buffer, data to send out + * @param rx the rx buffer, will be updated with data from the full-duplex transfer + * @param len the length of the tx and rx buffer + * @param spi_speed the speed to run spi at in Hz + * + */ +void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, + uint32_t spi_speed); + +/** + * Performs two back to back SPI transactions, first a read and then a write. + * + * NOTE: read_tx and write_tx are both expected to be in BIG ENDIAN + * + * @param fd the SPI file descriptor to transfer data over + * @param read_tx pointer to the buffer containing the address for reading + * @param write_tx pointer to the buffer containing the address + data for write + * @param read_rx the buffer our read response will be placed in + * @param spi_speed the speed to run spi at in Hz + */ +void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, const uint8_t* write_tx, + const uint8_t* read_rx, const uint32_t read_len, + const uint32_t write_len, uint32_t spi_speed); //refactor to take std::array, not raw pointers \ No newline at end of file From b90d380f59637819862dafe4b231ac5198ecaf1b Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 16:57:03 -0700 Subject: [PATCH 08/23] thunderloop --- src/software/embedded/thunderloop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index b8a440a683..7086c57882 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -41,7 +41,7 @@ extern "C" { if (g_motor_service) { - g_motor_service->resetMotorBoard(); + g_motor_service->reset(); } // by now g3log may have died due to the termination signal, so it isn't reliable @@ -118,7 +118,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, LOG(INFO) << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; - motor_service_ = std::make_unique(robot_constants, loop_hz); + motor_service_ = std::make_unique(robot_constants); g_motor_service = motor_service_.get(); motor_service_->setup(); LOG(INFO) << "THUNDERLOOP: Motor Service initialized!"; From 915f2a19ed0654a8e8aa5587922e3db032f06d13 Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 17:01:25 -0700 Subject: [PATCH 09/23] make enum --- src/software/util/make_enum/make_enum.hpp | 41 +++++++++++++++++------ 1 file changed, 31 insertions(+), 10 deletions(-) diff --git a/src/software/util/make_enum/make_enum.hpp b/src/software/util/make_enum/make_enum.hpp index 8b26be8bc2..eeccb9f970 100644 --- a/src/software/util/make_enum/make_enum.hpp +++ b/src/software/util/make_enum/make_enum.hpp @@ -50,22 +50,27 @@ return enum_size_##name; \ } \ template <> \ - constexpr auto reflective_enum::values() \ + constexpr auto& reflective_enum::values() \ { \ return enum_values_##name; \ } \ template <> \ - constexpr auto reflective_enum::valueNames() \ + constexpr auto& reflective_enum::valueNames() \ { \ return enum_value_names_##name; \ } \ inline std::ostream& operator<<(std::ostream& os, name value) \ { \ - /* This index lookup relies on the assumption that the enum does not manually */ \ - /* specify any values. If it did, the underlying integer of the given value */ \ - /* may be out of range of the vector of strings */ \ - os << reflective_enum::valueNames().at(static_cast(value)); \ + os << reflective_enum::nameOf(value); \ return os; \ + } \ + inline std::string operator+(const std::string& lhs, name rhs) \ + { \ + return lhs + std::string(reflective_enum::nameOf(rhs)); \ + } \ + inline std::string operator+(name lhs, const std::string& rhs) \ + { \ + return std::string(reflective_enum::nameOf(lhs)) + rhs; \ } namespace reflective_enum @@ -93,7 +98,7 @@ constexpr size_t size(); * @return an array with the values of the enum */ template -constexpr auto values(); +constexpr auto& values(); /** * Returns an array contain the string representations of each value @@ -102,7 +107,23 @@ constexpr auto values(); * @return an array with the names of the values in the enum */ template -constexpr auto valueNames(); +constexpr auto& valueNames(); + +/** + * Returns the string representation of the given enum value. + * + * @param value the enum value to get the name of + * + * @return the string representation of the enum value + */ +template +constexpr std::string_view nameOf(const E value) +{ + // This index lookup relies on the assumption that the enum does not manually + // specify any values. If it did, the underlying integer of the given value + // may be out of range of the vector of strings. + return reflective_enum::valueNames().at(static_cast(value)); +} /** * Returns the enum value with the given string representation. @@ -112,7 +133,7 @@ constexpr auto valueNames(); * @return the enum value */ template -constexpr E fromName(const std::string value_name) +constexpr E fromName(const std::string_view value_name) { constexpr size_t enum_size = size(); constexpr auto enum_value_names = valueNames(); @@ -123,6 +144,6 @@ constexpr E fromName(const std::string value_name) return static_cast(i); } } - throw std::invalid_argument(value_name + " cannot be converted to enum value"); + throw std::invalid_argument("Name cannot be converted to enum value"); } } // namespace reflective_enum From 6b05e6f92e6592def0d6fbce0edba8bcb1592dfd Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 17:02:45 -0700 Subject: [PATCH 10/23] adjust tests --- .../stspin_motor_controller_test.cpp | 282 ++++++++++++++++++ .../embedded/services/robot_auto_test.cpp | 107 ------- 2 files changed, 282 insertions(+), 107 deletions(-) create mode 100644 src/software/embedded/motor_controller/stspin_motor_controller_test.cpp delete mode 100644 src/software/embedded/services/robot_auto_test.cpp diff --git a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp new file mode 100644 index 0000000000..80b657f781 --- /dev/null +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -0,0 +1,282 @@ +#include "software/embedded/motor_controller/stspin_motor_controller.h" + +#include +#include +#include +#include +#include + +#include "software/logger/logger.h" +#include "software/logger/network_logger.h" + +// Flag to indicate when a stop has been requested (e.g. via Ctrl+C) +static volatile std::sig_atomic_t g_stop_requested = false; + +class StSpinMotorControllerTest +{ + public: + struct CommandLineArgs + { + struct PidOption + { + std::optional kp; + std::optional ki; + }; + + bool help = false; + std::string mode = "speed"; + int setpoint_duration_ms = 2000; + std::vector setpoints; + std::unordered_set enabled_motors; + PidOption torque_pid; + PidOption flux_pid; + PidOption speed_pid; + }; + + explicit StSpinMotorControllerTest(CommandLineArgs args) : args_(std::move(args)) {} + + void runMotorTest() const + { + LOG(INFO) << "Running Motor Test"; + + std::stringstream ss; + for (const MotorIndex motor : args_.enabled_motors) + { + ss << motor << " "; + } + LOG(INFO) << "Enabled motors: " << ss.str(); + + const auto motor_controller = std::make_unique( + robot_constants::createRobotConstants()); + motor_controller->setup(); + + LOG(INFO) << "Motor controller setup complete"; + + for (const MotorIndex motor : args_.enabled_motors) + { + if (args_.torque_pid.kp && args_.torque_pid.ki) + { + motor_controller->sendAndReceiveFrame( + motor, SetPidTorqueKpKiFrame{.kp = args_.torque_pid.kp.value(), + .ki = args_.torque_pid.ki.value()}); + } + + if (args_.flux_pid.kp && args_.flux_pid.ki) + { + motor_controller->sendAndReceiveFrame( + motor, SetPidFluxKpKiFrame{.kp = args_.flux_pid.kp.value(), + .ki = args_.flux_pid.ki.value()}); + } + + if (args_.speed_pid.kp && args_.speed_pid.ki) + { + motor_controller->sendAndReceiveFrame( + motor, SetPidSpeedKpKiFrame{.kp = args_.speed_pid.kp.value(), + .ki = args_.speed_pid.ki.value()}); + } + } + + LOG(INFO) << "PID configuration complete"; + + LOG(INFO) << "Running " << args_.mode << " profile"; + + for (const int16_t setpoint : args_.setpoints) + { + if (g_stop_requested) + { + break; + } + + StSpinMotorController::OutgoingFrame command_frame; + if (args_.mode == "speed") + { + command_frame = SetTargetSpeedFrame{ + .motor_enabled = true, + .motor_target_speed_rpm = setpoint, + }; + } + else if (args_.mode == "torque") + { + command_frame = SetTargetTorqueFrame{ + .motor_enabled = true, + .motor_target_torque = setpoint, + }; + } + + for (const MotorIndex motor : args_.enabled_motors) + { + motor_controller->sendAndReceiveFrame(motor, command_frame); + } + + auto start_time = std::chrono::steady_clock::now(); + auto duration = std::chrono::milliseconds(args_.setpoint_duration_ms); + while (std::chrono::steady_clock::now() - start_time < duration) + { + if (g_stop_requested) + { + break; + } + + for (const MotorIndex motor : args_.enabled_motors) + { + motor_controller->sendMotorStatusToPlotJuggler(motor); + motor_controller->checkFaults(motor); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + } + + motor_controller->immediatelyDisable(); + + LOG(INFO) << "Motor Test Complete"; + } + + private: + CommandLineArgs args_; +}; + +static std::vector parseSetpoints(const std::string& setpoints_str) +{ + std::vector setpoints; + std::stringstream ss(setpoints_str); + std::string item; + + while (std::getline(ss, item, ',')) + { + if (!item.empty()) + { + setpoints.push_back(static_cast(std::stoi(item))); + } + } + + return setpoints; +} + +static std::unordered_set parseEnabledMotors( + const std::string& enabled_motors_str) +{ + if (enabled_motors_str == "all") + { + constexpr auto all_motors = driveMotors(); + return {all_motors.begin(), all_motors.end()}; + } + + std::unordered_set enabled_motors; + std::stringstream ss(enabled_motors_str); + std::string item; + + while (std::getline(ss, item, ',')) + { + if (item == "fl") + { + enabled_motors.insert(MotorIndex::FRONT_LEFT); + } + else if (item == "fr") + { + enabled_motors.insert(MotorIndex::FRONT_RIGHT); + } + else if (item == "bl") + { + enabled_motors.insert(MotorIndex::BACK_LEFT); + } + else if (item == "br") + { + enabled_motors.insert(MotorIndex::BACK_RIGHT); + } + } + + return enabled_motors; +} + +int main(int argc, char** argv) +{ + NetworkLoggerSingleton::initializeLogger(0, true); + + StSpinMotorControllerTest::CommandLineArgs args; + + boost::program_options::options_description desc{"Options"}; + // clang-format off + desc.add_options() + ("help,h", boost::program_options::bool_switch(&args.help), "Show help") + ("mode", boost::program_options::value(&args.mode), + "Control mode: speed | torque") + ("setpoints", boost::program_options::value(), + "Comma-separated setpoints (required)") + ("setpoint_duration", boost::program_options::value(&args.setpoint_duration_ms), + "Duration to hold each setpoint in milliseconds") + ("enabled_motors", boost::program_options::value(), + "Motors to enable: 'all' (default) or comma-separated list (e.g. 'fl,fr,bl,br')") + ("torque_kp", boost::program_options::value(), "Torque PID Kp") + ("torque_ki", boost::program_options::value(), "Torque PID Ki") + ("flux_kp", boost::program_options::value(), "Flux PID Kp") + ("flux_ki", boost::program_options::value(), "Flux PID Ki") + ("speed_kp", boost::program_options::value(), "Speed PID Kp") + ("speed_ki", boost::program_options::value(), "Speed PID Ki"); + // clang-format on + + boost::program_options::variables_map vm; + boost::program_options::store(parse_command_line(argc, argv, desc), vm); + boost::program_options::notify(vm); + + if (args.help) + { + std::cout << desc << std::endl; + return 0; + } + + if (!vm.count("setpoints")) + { + std::cerr << "Error: setpoints not set" << std::endl; + return 1; + } + + if (vm.count("enabled_motors")) + { + args.enabled_motors = + parseEnabledMotors(vm.at("enabled_motors").as()); + } + else + { + args.enabled_motors = parseEnabledMotors("all"); + } + + args.setpoints = parseSetpoints(vm.at("setpoints").as()); + + if (vm.count("torque_kp")) + { + args.torque_pid.kp = vm.at("torque_kp").as(); + } + + if (vm.count("torque_ki")) + { + args.torque_pid.ki = vm.at("torque_ki").as(); + } + + if (vm.count("flux_kp")) + { + args.flux_pid.kp = vm.at("flux_kp").as(); + } + + if (vm.count("flux_ki")) + { + args.flux_pid.ki = vm.at("flux_ki").as(); + } + + if (vm.count("speed_kp")) + { + args.speed_pid.kp = vm.at("speed_kp").as(); + } + + if (vm.count("speed_ki")) + { + args.speed_pid.ki = vm.at("speed_ki").as(); + } + + std::signal(SIGINT, [](int) { g_stop_requested = true; }); + + StSpinMotorControllerTest stspin_motor_controller_test(args); + stspin_motor_controller_test.runMotorTest(); + + return 0; +} \ No newline at end of file diff --git a/src/software/embedded/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp deleted file mode 100644 index cc8abc541f..0000000000 --- a/src/software/embedded/services/robot_auto_test.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include "proto/message_translation/tbots_geometry.h" -#include "proto/primitive/primitive_msg_factory.h" -#include "shared/constants.h" -#include "shared/robot_constants.h" -#include "software/embedded/primitive_executor.h" -#include "software/embedded/services/motor.h" -#include "software/embedded/services/power.h" -#include "software/logger/network_logger.h" -#include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" - -extern "C" -{ -#include "tmc/ic/TMC4671/TMC4671.h" -#include "tmc/ic/TMC4671/TMC4671_Variants.h" -#include "tmc/ic/TMC6100/TMC6100.h" -} - -std::unique_ptr motor_service_; -std::unique_ptr power_service_; -robot_constants::RobotConstants robot_constants_; -int read_value; - -// SPI Chip Selects - -static const uint8_t CHIP_SELECT[] = {motor_service_->FRONT_LEFT_MOTOR_CHIP_SELECT, - motor_service_->FRONT_RIGHT_MOTOR_CHIP_SELECT, - motor_service_->BACK_LEFT_MOTOR_CHIP_SELECT, - motor_service_->BACK_RIGHT_MOTOR_CHIP_SELECT}; - -constexpr int ASCII_4671_IN_HEXADECIMAL = 0x34363731; -constexpr double THRESHOLD = 0.0001; -constexpr int DELAY_NS = 10000; -std::string runtime_dir = "/tmp/tbots/yellow_test"; - -int main(int argc, char** argv) -{ - LoggerSingleton::initializeLogger(runtime_dir, nullptr, false); - - LOG(INFO) << "Running on the Raspberry Pi!"; - - motor_service_ = std::make_unique( - robot_constants::createRobotConstants(), THUNDERLOOP_HZ); - - // Testing Motor board SPI transfer - for (uint8_t chip_select : CHIP_SELECT) - { - LOG(INFO) << "Checking motor: " << int(chip_select); - - // Check driver fault - if (!motor_service_->checkDriverFault(chip_select).drive_enabled) - { - LOG(WARNING) << "Detected Motor Fault"; - } - - motor_service_->writeIntToTMC4671(chip_select, TMC4671_CHIPINFO_ADDR, - 0x000000000); - read_value = - motor_service_->readIntFromTMC4671(chip_select, TMC4671_CHIPINFO_DATA); - - // Check if CHIPINFO_DATA returns 0x34363731 - if (read_value == ASCII_4671_IN_HEXADECIMAL) - { - LOG(INFO) << "SPI Transfer is successful"; - } - else - { - LOG(WARNING) << "SPI Transfer not successful"; - } - } - - motor_service_->resetMotorBoard(); - - // Testing Power board UART transfer - try - { - power_service_ = std::make_unique(); - - usleep(DELAY_NS); - - TbotsProto::PowerStatus power_status = - power_service_->poll(TbotsProto::PowerControl(), 0, 0, 0); - power_status = power_service_->poll(TbotsProto::PowerControl(), 0, 0, 0); - - if (abs(power_status.battery_voltage()) < THRESHOLD) - { - LOG(WARNING) << "Battery voltage is zero"; - } - else if (abs(power_status.capacitor_voltage()) < THRESHOLD) - { - LOG(WARNING) << "Capacitor voltage is zero"; - } - else if (abs(power_status.current_draw()) < THRESHOLD) - { - LOG(WARNING) << "Current draw is zero"; - } - else if (power_status.sequence_num() == 0) - { - LOG(WARNING) << "Sequence number is zero"; - } - } - catch (std::runtime_error& e) - { - LOG(WARNING) << "Unable to communicate with the power board"; - } - - return 0; -} From f2fcb94ec3a8827ae87ef2a3c6d303c80e885ac2 Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 17:03:04 -0700 Subject: [PATCH 11/23] trinamics --- .../motor_controller/tmc_motor_controller.cpp | 749 ++++++++++++++++++ .../motor_controller/tmc_motor_controller.h | 297 +++++++ 2 files changed, 1046 insertions(+) create mode 100644 src/software/embedded/motor_controller/tmc_motor_controller.cpp create mode 100644 src/software/embedded/motor_controller/tmc_motor_controller.h diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp new file mode 100644 index 0000000000..a917e6284f --- /dev/null +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -0,0 +1,749 @@ +#include "software/embedded/motor_controller/tmc_motor_controller.h" + + +#include "shared/constants.h" +#include "software/embedded/gpio/gpio_char_dev.h" +#include "software/embedded/spi_utils.h" +#include "software/logger/logger.h" + +extern "C" +{ +#include "tmc/ic/TMC4671/TMC4671.h" +#include "tmc/ic/TMC4671/TMC4671_Register.h" +#include "tmc/ic/TMC4671/TMC4671_Variants.h" +#include "tmc/ic/TMC6100/TMC6100.h" +} + +#include + +#include +#include + +extern "C" +{ + // We need a static pointer here, because trinamic externs the following two + // SPI binding functions that we need to interface with their API. + // + // The motor service exclusively calls the trinamic API which triggers these + // functions. The motor service will set this variable in the constructor. + static TmcMotorController* g_motor_controller = nullptr; + + uint8_t tmc4671_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) + { + return g_motor_controller->tmc4671ReadWriteByte(motor, data, last_transfer); + } + + uint8_t tmc6100_readwriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) + { + return g_motor_controller->tmc6100ReadWriteByte(motor, data, last_transfer); + } +} + +TmcMotorController::TmcMotorController() + : spi_demux_select_0_(std::make_unique( + SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO, GpioDirection::OUTPUT, GpioState::LOW)), + spi_demux_select_1_(std::make_unique( + SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO, GpioDirection::OUTPUT, GpioState::LOW)), + driver_control_enable_gpio_(std::make_unique( + DRIVER_CONTROL_ENABLE_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), + reset_gpio_(std::make_unique(MOTOR_DRIVER_RESET_GPIO, + GpioDirection::OUTPUT, GpioState::HIGH)) +{ + openSpiFileDescriptor(MotorIndex::FRONT_LEFT); + openSpiFileDescriptor(MotorIndex::FRONT_RIGHT); + openSpiFileDescriptor(MotorIndex::BACK_LEFT); + openSpiFileDescriptor(MotorIndex::BACK_RIGHT); + openSpiFileDescriptor(MotorIndex::DRIBBLER); + + // Make this instance available to the static functions above + g_motor_controller = this; +} + +int TmcMotorController::readThenWriteVelocity(const MotorIndex motor, + const int target_velocity) +{ + if (motor == MotorIndex::DRIBBLER) + { + const int velocity_erpm = readThenWriteValue( + motor, TMC4671_PID_VELOCITY_ACTUAL, TMC4671_PID_VELOCITY_TARGET, + target_velocity * DRIBBLER_MOTOR_ELECTRICAL_RPM_PER_MECHANICAL_RPM); + return velocity_erpm * DRIBBLER_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM; + } + else + { + const int velocity_erpm = readThenWriteValue( + motor, TMC4671_PID_VELOCITY_ACTUAL, TMC4671_PID_VELOCITY_TARGET, + target_velocity * DRIVE_MOTOR_ELECTRICAL_RPM_PER_MECHANICAL_RPM); + return velocity_erpm * DRIVE_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM; + } +} + +void TmcMotorController::updateEuclideanVelocity( + EuclideanSpace_t target_euclidean_velocity) +{ +} + + +void TmcMotorController::writeToDriverOrDieTrying(const uint8_t motor, + const uint8_t address, + const int32_t value) +{ + int num_retries_left = NUM_RETRIES_SPI; + int read_value = 0; + + // The SPI lines have a lot of noise, and sometimes a transfer will fail + // randomly. So we retry a few times before giving up. + while (num_retries_left > 0) + { + tmc6100_writeInt(motor, address, value); + read_value = tmc6100_readInt(motor, address); + if (read_value == value) + { + return; + } + LOG(DEBUG) << "SPI Transfer to Driver Failed, retrying..."; + num_retries_left--; + } + + // If we get here, we have failed to write to the driver. We reset + // the chip to clear any bad values we just wrote and crash so everything stops. + reset_gpio_->setValue(GpioState::LOW); + CHECK(read_value == value) << "Couldn't write " << value + << " to the TMC6100 at address " << address + << " at address " << static_cast(address) + << " on motor " << static_cast(motor) + << " received: " << read_value; +} + +void TmcMotorController::writeToControllerOrDieTrying(const MotorIndex motor, + const uint8_t address, + const int32_t value) +{ + int num_retries_left = NUM_RETRIES_SPI; + int read_value = 0; + + // The SPI lines have a lot of noise, and sometimes a transfer will fail + // randomly. So we retry a few times before giving up. + while (num_retries_left > 0) + { + tmc4671_writeInt(CHIP_SELECTS.at(motor), address, value); + read_value = tmc4671_readInt(CHIP_SELECTS.at(motor), address); + if (read_value == value) + { + return; + } + LOG(DEBUG) << "SPI Transfer to Controller Failed, retrying..."; + num_retries_left--; + } +} + +void TmcMotorController::setup() +{ + reset_gpio_->setValue(GpioState::LOW); + usleep(MICROSECONDS_PER_MILLISECOND * 100); + + reset_gpio_->setValue(GpioState::HIGH); + usleep(MICROSECONDS_PER_MILLISECOND * 100); + + for (const MotorIndex motor : reflective_enum::values()) + { + motor_faults_[motor] = MotorFaultIndicator(); + num_motor_fault_checks_[motor] = 0; + } + + for (const MotorIndex motor : reflective_enum::values()) + { + LOG(INFO) << "Clearing RESET for " << motor; + tmc6100_writeInt(CHIP_SELECTS.at(motor), TMC6100_GSTAT, 0x00000001); + encoder_calibrated_[motor] = false; + } + + // Drive Motor Setup + for (const MotorIndex motor : driveMotors()) + { + setupDriveMotor(motor); + } + + // Dribbler Motor Setup + startDriver(MotorIndex::DRIBBLER); + checkFaults(MotorIndex::DRIBBLER); + startController(MotorIndex::DRIBBLER, true); + tmc4671_setTargetVelocity(DRIBBLER_MOTOR_CHIP_SELECT, 0); + + checkEncoderConnections(); + + // calibrate the encoders + for (const MotorIndex motor : driveMotors()) + { + startEncoderCalibration(motor); + } + + sleep(1); + + for (const MotorIndex motor : driveMotors()) + { + endEncoderCalibration(motor); + } + + auto motors = driveMotors(); + bool has_encoders_calibrated = + std::accumulate(motors.begin(), motors.end(), false, + [&](const bool& acc, const MotorIndex motor) + { return acc || encoder_calibrated_[motor]; }); + CHECK(has_encoders_calibrated) + << "Running without encoder calibration can cause serious harm, exiting"; +} + +void TmcMotorController::configurePWM(const MotorIndex motor) +{ + LOG(INFO) << "Configuring PWM for motor " << static_cast(motor); + // Please read the header file and the datasheet for more info + writeToControllerOrDieTrying(motor, TMC4671_PWM_POLARITIES, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_PWM_MAXCNT, 0x00000F9F); + writeToControllerOrDieTrying(motor, TMC4671_PWM_BBM_H_BBM_L, 0x00002828); + writeToControllerOrDieTrying(motor, TMC4671_PWM_SV_CHOP, 0x00000107); +} + +void TmcMotorController::configureDrivePI(const MotorIndex motor) +{ + LOG(INFO) << "Configuring Drive PI for motor " << static_cast(motor); + // Please read the header file and the datasheet for more info + // These values were calibrated using the TMC-IDE + writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 67109376); + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 67109376); + writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_P_VELOCITY_I, 52428800); + + // Explicitly disable the position controller + writeToControllerOrDieTrying(motor, TMC4671_PID_POSITION_P_POSITION_I, 0); + + writeToControllerOrDieTrying(motor, TMC4671_PIDOUT_UQ_UD_LIMITS, 32767); + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 2500); + writeToControllerOrDieTrying(motor, TMC4671_PID_ACCELERATION_LIMIT, 1000); + + writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_LIMIT, 45000); + + tmc4671_switchToMotionMode(CHIP_SELECTS.at(motor), TMC4671_MOTION_MODE_VELOCITY); +} + +void TmcMotorController::configureDribblerPI(const MotorIndex motor) +{ + LOG(INFO) << "Configuring Dribbler PI for motor " << static_cast(motor); + // Please read the header file and the datasheet for more info + // These values were calibrated using the TMC-IDE + writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 39337600); + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 39333600); + writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_P_VELOCITY_I, 2621448); + + // Explicitly disable the position controller + writeToControllerOrDieTrying(motor, TMC4671_PID_POSITION_P_POSITION_I, 0); + + writeToControllerOrDieTrying(motor, TMC4671_PIDOUT_UQ_UD_LIMITS, 32767); + // TODO (#2677) support MAX_FORCE mode. This value can go up to 4.8 amps but we set it + // to 2 for now (sufficient for INDEFINITE mode). + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 4000); + writeToControllerOrDieTrying(motor, TMC4671_PID_ACCELERATION_LIMIT, 40000); + writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_LIMIT, 15000); +} + +void TmcMotorController::configureADC(const MotorIndex motor) +{ + LOG(INFO) << "Configuring ADC for motor " << static_cast(motor); + // ADC configuration + writeToControllerOrDieTrying(motor, TMC4671_ADC_I_SELECT, 0x18000100); + writeToControllerOrDieTrying(motor, TMC4671_dsADC_MDEC_B_MDEC_A, 0x014E014E); + + // These values have been calibrated for the TI INA240 current sense amplifier. + // The scaling is also set to work with both the drive and dribbler motors. + // + // If you wish to use the TMC4671+TMC6100-BOB you can use the following values, + // that work for the AD8418 current sense amplifier + // + // TMC4671_ADC_I0_SCALE_OFFSET = 0x010081DD + // TMC4671_ADC_I1_SCALE_OFFSET = 0x0100818E + // + writeToControllerOrDieTrying(motor, TMC4671_ADC_I0_SCALE_OFFSET, 0x000981DD); + writeToControllerOrDieTrying(motor, TMC4671_ADC_I1_SCALE_OFFSET, 0x0009818E); +} + +void TmcMotorController::configureEncoder(const MotorIndex motor) +{ + LOG(INFO) << "Configuring Encoder for motor " << static_cast(motor); + // ABN encoder settings + writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_MODE, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_PPR, 0x00001000); +} + +void TmcMotorController::configureHall(const MotorIndex motor) +{ + LOG(INFO) << "Configuring Hall for motor " << static_cast(motor); + // Digital hall settings + writeToControllerOrDieTrying(motor, TMC4671_HALL_MODE, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_HALL_PHI_E_PHI_M_OFFSET, 0x00000000); + + // Feedback selection + writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, TMC4671_PHI_E_HALL); + writeToControllerOrDieTrying(motor, TMC4671_VELOCITY_SELECTION, + TMC4671_VELOCITY_PHI_E_HAL); +} + +const MotorFaultIndicator& TmcMotorController::checkFaults(const MotorIndex motor) +{ + num_motor_fault_checks_[motor]++; + + // To reduce the number of SPI transactions, we only check for faults every + // MOTOR_FAULT_CHECK_INTERVAL calls and return the cached faults otherwise. + if (num_motor_fault_checks_.at(motor) % MOTOR_FAULT_CHECK_INTERVAL == 0) + { + return motor_faults_.at(motor); + } + + MotorFaultIndicator& motor_faults = motor_faults_.at(motor); + motor_faults.faults.clear(); + + const int gstat = tmc6100_readInt(CHIP_SELECTS.at(motor), TMC6100_GSTAT); + const std::bitset<32> gstat_bitset(gstat); + + if (gstat_bitset.any()) + { + LOG(WARNING) << "======= Faults For Motor " << motor << "======="; + } + + if (gstat_bitset[0]) + { + LOG(WARNING) + << "Indicates that the IC has been reset. All registers have been cleared to reset values." + << "Attention: DRV_EN must be high to allow clearing reset"; + motor_faults.faults.insert(TbotsProto::MotorFault::RESET); + } + + if (gstat_bitset[1]) + { + LOG(WARNING) + << "drv_otpw : Indicates, that the driver temperature has exceeded overtemperature prewarning-level." + << "No action is taken. This flag is latched."; + motor_faults.faults.insert( + TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE_PREWARNING); + } + + if (gstat_bitset[2]) + { + LOG(WARNING) + << "drv_ot: Indicates, that the driver has been shut down due to overtemperature." + << "This flag can only be cleared when the temperature is below the limit again." + << "It is latched for information."; + motor_faults.faults.insert(TbotsProto::MotorFault::DRIVER_OVERTEMPERATURE); + } + + if (gstat_bitset[3]) + { + LOG(WARNING) << "uv_cp: Indicates an undervoltage on the charge pump." + << "The driver is disabled during undervoltage." + << "This flag is latched for information."; + motor_faults.faults.insert(TbotsProto::MotorFault::UNDERVOLTAGE_CHARGEPUMP); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[4]) + { + LOG(WARNING) << "shortdet_u: Short to GND detected on phase U." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert( + TbotsProto::MotorFault::PHASE_U_SHORT_COUNTER_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[5]) + { + LOG(WARNING) << "s2gu: Short to GND detected on phase U." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_U_SHORT_TO_GND_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[6]) + { + LOG(WARNING) << "s2vsu: Short to VS detected on phase U." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_U_SHORT_TO_VS_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[8]) + { + LOG(WARNING) << "shortdet_v: V short counter has triggered at least once."; + motor_faults.faults.insert( + TbotsProto::MotorFault::PHASE_V_SHORT_COUNTER_DETECTED); + } + + if (gstat_bitset[9]) + { + LOG(WARNING) << "s2gv: Short to GND detected on phase V." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_V_SHORT_TO_GND_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[10]) + { + LOG(WARNING) << "s2vsv: Short to VS detected on phase V." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_V_SHORT_TO_VS_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[12]) + { + LOG(WARNING) << "shortdet_w: short counter has triggered at least once."; + motor_faults.faults.insert( + TbotsProto::MotorFault::PHASE_W_SHORT_COUNTER_DETECTED); + } + + if (gstat_bitset[13]) + { + LOG(WARNING) << "s2gw: Short to GND detected on phase W." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_TO_GND_DETECTED); + motor_faults.drive_enabled = false; + } + + if (gstat_bitset[14]) + { + LOG(WARNING) << "s2vsw: Short to VS detected on phase W." + << "The driver becomes disabled until flag becomes cleared."; + motor_faults.faults.insert(TbotsProto::MotorFault::PHASE_W_SHORT_TO_VS_DETECTED); + motor_faults.drive_enabled = false; + } + + return motor_faults; +} + +int TmcMotorController::readThenWriteValue(const MotorIndex motor, + const uint8_t read_addr, + const uint8_t write_addr, const int write_data) +{ + spi_demux_select_0_->setValue(GpioState::HIGH); + spi_demux_select_1_->setValue(GpioState::LOW); + + // ensure tx_ and rx_ are cleared + memset(read_tx_, 0, 5); + memset(write_tx_, 0, 5); + memset(read_rx_, 0, 5); + + // Trinamic transactions looks like this: + // + - - - + - - - + - - - + - - - + - - - + + // | ADDR | DATA | + // + - - - + - - - + - - - + - - - + - - - + + // 0 1 2 3 4 + // Also it is in BIG Endian, therefore MSB is leftmost bit of 0. + // For a write, MSB must be 1, for read, MSB must be 0 + // https://github.com/trinamic/TMC-API/blob/master/tmc/ic/TMC4671/TMC4671.c + read_tx_[0] = read_addr & 0x7f; + write_tx_[0] = write_addr | 0x80; + + // Convert from little endian to big endian + for (int i = 3; i >= 0; i--) + { + uint8_t byte_to_copy = (uint8_t)(0xff & (write_data >> 8 * i)); + write_tx_[4 - i] = byte_to_copy; + } + + readThenWriteSpiTransfer(file_descriptors_[CHIP_SELECTS.at(motor)], read_tx_, + write_tx_, read_rx_, TMC_CMD_MSG_SIZE, TMC_CMD_MSG_SIZE, + TMC4671_SPI_SPEED); + + int32_t value = read_rx_[0]; + for (int i = 1; i < 5; i++) + { + value <<= 8; + value |= read_rx_[i]; + } + return value; +} + +void TmcMotorController::openSpiFileDescriptor(const MotorIndex motor_index) +{ + file_descriptors_[CHIP_SELECTS.at(motor_index)] = + open(SPI_PATHS.at(motor_index), O_RDWR); + CHECK(file_descriptors_[CHIP_SELECTS.at(motor_index)] >= 0) + << "can't open device: " << motor_index << "error: " << strerror(errno); + + int ret = ioctl(file_descriptors_[CHIP_SELECTS.at(motor_index)], SPI_IOC_WR_MODE32, + &SPI_MODE); + CHECK(ret != -1) << "can't set spi mode for: " << motor_index + << "error: " << strerror(errno); + + ret = ioctl(file_descriptors_[CHIP_SELECTS.at(motor_index)], SPI_IOC_WR_BITS_PER_WORD, + &SPI_BITS); + CHECK(ret != -1) << "can't set bits_per_word for: " << motor_index + << "error: " << strerror(errno); + + ret = ioctl(file_descriptors_[CHIP_SELECTS.at(motor_index)], SPI_IOC_WR_MAX_SPEED_HZ, + &MAX_SPI_SPEED_HZ); + CHECK(ret != -1) << "can't set spi max speed hz for: " << motor_index + << "error: " << strerror(errno); +} + +void TmcMotorController::setupDriveMotor(const MotorIndex motor) +{ + startDriver(motor); + checkFaults(motor); + // Start all the controllers as drive motor controllers + startController(motor, false); + tmc4671_setTargetVelocity(CHIP_SELECTS.at(motor), 0); +} + +void TmcMotorController::startDriver(const MotorIndex motor) +{ + uint8_t motor_cs = CHIP_SELECTS.at(motor); + + // Set the drive strength to 0, the weakest it can go as recommended + // by the TMC4671-TMC6100-BOB datasheet. + int32_t current_drive_conf = tmc6100_readInt(motor_cs, TMC6100_DRV_CONF); + writeToDriverOrDieTrying(motor_cs, TMC6100_DRV_CONF, + current_drive_conf & (~TMC6100_DRVSTRENGTH_MASK)); + writeToDriverOrDieTrying(motor_cs, TMC6100_GCONF, 0x40); + + // All default but updated SHORTFILTER to 2us to avoid false positive shorts + // detection. + writeToDriverOrDieTrying(motor_cs, TMC6100_SHORT_CONF, 0x13020606); + + LOG(DEBUG) << "Driver " << motor << " accepted conf"; +} + +void TmcMotorController::startController(const MotorIndex motor, const bool dribbler) +{ + // Read the chip ID to validate the SPI connection + tmc4671_writeInt(CHIP_SELECTS.at(motor), TMC4671_CHIPINFO_ADDR, 0x000000000); + int chip_id = tmc4671_readInt(CHIP_SELECTS.at(motor), TMC4671_CHIPINFO_DATA); + + CHECK(0x34363731 == chip_id) + << "The TMC4671 of motor " << motor << " is not responding"; + + LOG(DEBUG) << "Controller " << motor << " online, responded with: " << chip_id; + + // Configure common controller params + configurePWM(motor); + configureADC(motor); + + if (dribbler) + { + // Configure to brushless DC motor with 1 pole pair + writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, + 0x00030000 + DRIBBLER_MOTOR_NUM_POLE_PAIRS); + configureHall(motor); + + configureDribblerPI(motor); + } + else + { + // Configure to brushless DC motor with 8 pole pairs + writeToControllerOrDieTrying(motor, TMC4671_MOTOR_TYPE_N_POLE_PAIRS, + 0x00030000 + DRIVE_MOTOR_NUM_POLE_PAIRS); + configureEncoder(motor); + } +} + +uint8_t TmcMotorController::tmc4671ReadWriteByte(const uint8_t motor, const uint8_t data, + const uint8_t last_transfer) +{ + spi_demux_select_0_->setValue(GpioState::HIGH); + spi_demux_select_1_->setValue(GpioState::LOW); + return readWriteByte(motor, data, last_transfer, TMC4671_SPI_SPEED); +} + +uint8_t TmcMotorController::tmc6100ReadWriteByte(const uint8_t motor, const uint8_t data, + const uint8_t last_transfer) +{ + spi_demux_select_0_->setValue(GpioState::LOW); + spi_demux_select_1_->setValue(GpioState::HIGH); + return readWriteByte(motor, data, last_transfer, TMC6100_SPI_SPEED); +} + +uint8_t TmcMotorController::readWriteByte(const uint8_t motor, const uint8_t data, + const uint8_t last_transfer, + const uint32_t spi_speed) +{ + uint8_t ret_byte = 0; + + if (!transfer_started_) + { + memset(tx_, 0, sizeof(tx_)); + memset(rx_, 0, sizeof(rx_)); + position_ = 0; + + if (data & TMC_WRITE_BIT) + { + // If the transfer started and its a write operation, + // set the appropriate flags. + currently_reading_ = false; + currently_writing_ = true; + } + else + { + // The first byte should contain the address on a read operation. + // Trigger a transfer (1 byte) and buffer the response (4 bytes) + tx_[position_] = data; + spiTransfer(file_descriptors_[motor], tx_, rx_, 5, spi_speed); + + currently_reading_ = true; + currently_writing_ = false; + } + + transfer_started_ = true; + } + + if (currently_writing_) + { + // Buffer the data to send out when last_transfer is true. + tx_[position_++] = data; + } + + if (currently_reading_) + { + // If we are reading, we just need to return the buffered data + // byte by byte. + ret_byte = rx_[position_++]; + } + + if (currently_writing_ && last_transfer) + { + // we have all the bytes for this transfer, lets trigger the transfer and + // reset state + spiTransfer(file_descriptors_[motor], tx_, rx_, 5, spi_speed); + transfer_started_ = false; + } + + if (currently_reading_ && last_transfer) + { + // when reading, if last transfer is true, we just need to reset state + transfer_started_ = false; + } + + return ret_byte; +} + +void TmcMotorController::checkEncoderConnections() +{ + LOG(INFO) << "Starting encoder connection check!"; + + std::unordered_map calibrated_motors; + std::unordered_map initial_velocities; + + for (const MotorIndex motor : driveMotors()) + { + calibrated_motors[motor] = false; + + // read back current velocity + initial_velocities[motor] = + tmc4671_readInt(CHIP_SELECTS.at(motor), TMC4671_ABN_DECODER_COUNT); + + // open loop mode can be used without an encoder, set open loop phi positive + // direction + writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_MODE, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, + TMC4671_PHI_E_OPEN_LOOP); + writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_ACCELERATION, 0x0000003C); + + // represents effective voltage applied to the motors (% voltage) + writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000799); + + // uq_ud_ext mode + writeToControllerOrDieTrying(motor, TMC4671_MODE_RAMP_MODE_MOTION, 0x00000008); + + // 10 RPM + writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x0000000A); + } + + for (int num_iterations = 0; + num_iterations < 10 && + std::any_of(calibrated_motors.begin(), calibrated_motors.end(), + [](std::pair calibration_status_pair) + { return !calibration_status_pair.second; }); + ++num_iterations) + { + for (const MotorIndex motor : driveMotors()) + { + if (calibrated_motors[motor]) + { + continue; + } + // now read back the velocity + int read_back_velocity = + tmc4671_readInt(CHIP_SELECTS.at(motor), TMC4671_ABN_DECODER_COUNT); + LOG(INFO) << motor << " read back: " << read_back_velocity + << " and initially read: " << initial_velocities[motor]; + + if (read_back_velocity != initial_velocities[motor]) + { + calibrated_motors[motor] = true; + } + } + + // sleep for 100 milliseconds + usleep(MICROSECONDS_PER_MILLISECOND * 100); + } + + bool calibrated = true; + for (const MotorIndex motor : driveMotors()) + { + if (!calibrated_motors[motor]) + { + calibrated = false; + LOG(WARNING) << "Encoder calibration check failure. " << motor + << " did not change as expected"; + } + } + if (!calibrated) + { + LOG(FATAL) + << "Encoder calibration check failure. Not all encoders responded as expected"; + } + + // stop all motors, reset back to velocity control mode + for (const MotorIndex motor : driveMotors()) + { + writeToControllerOrDieTrying(motor, TMC4671_OPENLOOP_VELOCITY_TARGET, 0x00000000); + tmc4671_switchToMotionMode(CHIP_SELECTS.at(motor), TMC4671_MOTION_MODE_VELOCITY); + } + + LOG(INFO) << "All encoders appear to be connected!"; +} + +void TmcMotorController::reset() +{ + reset_gpio_->setValue(GpioState::LOW); +} + +void TmcMotorController::startEncoderCalibration(const MotorIndex motor) +{ + LOG(WARNING) << "Calibrating the encoder, ensure the robot is lifted off the ground"; + + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 0x000003E8); + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_P_TORQUE_I, 0x01000100); + writeToControllerOrDieTrying(motor, TMC4671_PID_FLUX_P_FLUX_I, 0x01000100); + + writeToControllerOrDieTrying(motor, TMC4671_MODE_RAMP_MODE_MOTION, 0x00000008); + writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_PHI_E_PHI_M_OFFSET, + 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, 0x00000001); + writeToControllerOrDieTrying(motor, TMC4671_PHI_E_EXT, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000FFF); +} + +void TmcMotorController::endEncoderCalibration(const MotorIndex motor) +{ + LOG(WARNING) << "Calibrating the encoder, wheels may move"; + + writeToControllerOrDieTrying(motor, TMC4671_ABN_DECODER_COUNT, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_UQ_UD_EXT, 0x00000000); + writeToControllerOrDieTrying(motor, TMC4671_PHI_E_SELECTION, TMC4671_PHI_E_ABN); + + encoder_calibrated_[motor] = true; + + configureDrivePI(motor); +} + +void TmcMotorController::immediatelyDisable() +{ + driver_control_enable_gpio_->setValue(GpioState::LOW); +} \ No newline at end of file diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h new file mode 100644 index 0000000000..8cb3c0889b --- /dev/null +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -0,0 +1,297 @@ +#pragma once +#include "software/embedded/gpio/gpio.h" +#include "software/embedded/motor_controller/motor_controller.h" +#include "software/embedded/motor_controller/motor_fault_indicator.h" +#include "software/embedded/motor_controller/motor_index.h" + +/** + * Motor controller for interfacing with our 5th generation Trinamic motor + * controllers and drivers. + * + * TMC4671 is the controller and TMC6100 is the driver. We use the TMC4671's SPI + * interface to configure the controller and driver, read/set motor velocities, etc. + */ +class TmcMotorController : public MotorController +{ + public: + TmcMotorController(); + + void setup() override; + + void reset() override; + + const MotorFaultIndicator& checkFaults(MotorIndex motor) override; + + void immediatelyDisable() override; + + int readThenWriteVelocity(MotorIndex motor, int target_velocity) override; + + void updateEuclideanVelocity(EuclideanSpace_t target_euclidean_velocity) override; + + /** + * Trinamic API binding, sets spi_demux_select_0|1 pins appropriately and + * calls readWriteByte. + * + * Both the TMC4671 (the controller) and the TMC6100 (the driver) respect + * the same SPI interface. So when we bind the API, we can use the same + * readWriteByte function, provided that the chip select pin is turning on + * the right chip. + * + * Each TMC4671 controller, TMC6100 driver and encoder group have their chip + * selects coming in from a demux (see diagram below). The demux is controlled + * by two bits {spi_demux_select_0, spi_demux_select_1}. If the bits are + * 10 the TMC4671 is selected, when the select bits are 01 the TMC6100 is + * selected and when they are 11 the encoder is selected. 00 disconnects all + * 3 chips. + * + * + * FRONT LEFT MOTOR + * CONTROLLER + DRIVER + ENCODER + * + * ┌───────┐ ┌───────────────┐ + * │ │ │ │ + * │ 2:4 │ 10 │ ┌─────────┐ │ + * │ ├────────┼──►TMC4671 │ │ B0 + * FRONT_LEFT_CS │ DEMUX │ │ └─────────┘ │ + * ───────────────► │ │ │ + * │ │ 01 │ ┌─────────┐ │ + * │ ├────────┼──►TMC6100 │ │ B1 + * │ │ │ └─────────┘ │ + * │ │ │ │ + * │ │ 11 │ ┌─────────┐ │ + * │ ├────────┼──►ENCODER │ │ B2 + * │ │ │ └─────────┘ │ + * └───▲───┘ │ │ + * │ └───────────────┘ + * │ + * spi_demux_sel_0 & 1 + * + * + * @param motor Which motor to talk to (in our case, the chip select) + * @param data The data to send + * @param last_transfer The last transfer of uint8_t data for this transaction. + * @return A byte read from the trinamic chip + */ + uint8_t tmc4671ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer); + uint8_t tmc6100ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer); + + private: + /** + * Opens SPI file descriptor + * + * @param motor_index The index of the motor to open + */ + void openSpiFileDescriptor(MotorIndex motor_name); + + /** + * A function which is written in the same style as the rest of the Trinamic API. + * This will trigger two SPI transactions back to back, reading a value and then + * writing a value for a specific motor + * + * @param motor The motor we want to read & write from + * @param read_addr the address of the register to read + * @param write_addr the address of the register to write + * @param write_data the data to write + * @return the value read from the trinamic controller + */ + int readThenWriteValue(MotorIndex motor, uint8_t read_addr, uint8_t write_addr, + int write_data); + + /** + * A lot of initialization parameters are necessary to function. Even if + * there is a single bit error, we can risk frying the motor driver or + * controller. + * + * The following functions can be used to setup initialization params + * that _must_ be set to continue. A failed call will crash the program + * + * @param motor Which motor to talk to (in our case, the chip select) + * @param address The address to send data to + * @param value The value to write + * + */ + void writeToControllerOrDieTrying(MotorIndex motor, uint8_t address, int32_t value); + void writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int32_t value); + + /** + * Sets up motor as drive motor controllers + * + * @param motor drive motor number + */ + void setupDriveMotor(MotorIndex motor); + + /** + * Calls the configuration functions below in the right sequence + * + * @param motor The motor setup the driver/controller for + * @param dribbler If true, configures the motor to be a dribbler + */ + void startDriver(MotorIndex motor); + void startController(MotorIndex motor, bool dribbler); + + /** + * Configuration settings + * + * These values were determined by reading the datasheets and user manual + * here: https://www.trinamic.com/support/eval-kits/details/tmc4671-tmc6100-bob/ + * + * If you are planning to change these settings, I highly recommend that you + * plug the motor + encoder pair in the TMC-IDE and use the TMC4671 EVAL + * with the TMC6100 EVAL to get the motor spinning. + * + * Then using the exported registers as a baseline, you can use the + * runOpenLoopCalibrationRoutine and plot the generated csvs. These csvs capture the + * data for encoder calibration and adc configuration, the two most important steps + * for the motor to work. Page 143 (title Setup Guidelines) of the TMC4671 is very + * useful. + * + * @param motor The motor to configure (the same value as the chip select) + */ + void configurePWM(MotorIndex motor); + void configureDribblerPI(MotorIndex motor); + void configureDrivePI(MotorIndex motor); + void configureADC(MotorIndex motor); + void configureEncoder(MotorIndex motor); + void configureHall(MotorIndex motor); + + /** + * Trinamic API Binding function + * + * @param motor Which motor to talk to (in our case, the chip select) + * @param data The data to send + * @param last_transfer The last transfer of uint8_t data for this transaction. + * @param spi_speed The speed to run spi at + * + * @return A byte read from the trinamic chip + */ + uint8_t readWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer, + uint32_t spi_speed); + + /* + * For FOC to work, the controller needs to know the electical angle of the rotor + * relative to the mechanical angle of the rotor. In an incremental-encoder-only + * setup, we can energize the motor coils so that the rotor locks itself along + * one of its pole-pairs, allowing us to reset the encoder. + * + * WARNING: Do not try to spin the motor without initializing the encoder! + * The motor can overheat if the TMC4671 doesn't auto shut-off. + * + * There are some safety checks to ensure that the encoder is + * initialized, do not tamper with them. You have been warned. + * + * @param motor The motor to initialize the encoder for + */ + void startEncoderCalibration(MotorIndex motor); + void endEncoderCalibration(MotorIndex motor); + + /** + * Spin each drive motor in open loop mode to check if the encoder + * is responding as expected. Allows us to do a basic test of whether + * the encoder is physically connected to the motor board. + * + * Leaves the motors connected in MOTION_MODE_VELOCITY + */ + void checkEncoderConnections(); + + // Select between driver and controller gpio + std::unique_ptr spi_demux_select_0_; + std::unique_ptr spi_demux_select_1_; + + // Enable driver gpio + std::unique_ptr driver_control_enable_gpio_; + std::unique_ptr reset_gpio_; + + // Transfer Buffers for spiTransfer + uint8_t tx_[5] = {}; + uint8_t rx_[5] = {}; + + // Transfer Buffers for readThenWriteSpiTransfer + uint8_t write_tx_[5] = {}; + uint8_t read_tx_[5] = {}; + uint8_t read_rx_[5] = {}; + + // Transfer State + bool transfer_started_ = false; + bool currently_writing_ = false; + bool currently_reading_ = false; + uint8_t position_ = 0; + + // Tracks whether each motor's encoder has been calibrated + std::unordered_map encoder_calibrated_; + + // Cached faults for each motor + std::unordered_map motor_faults_; + + // Number of times we've polled each motor for faults, + // used to determine when to actually check for faults again + std::unordered_map num_motor_fault_checks_; + + // The interval at which to check for motor faults, in number of polls. + // We avoid checking faults at every poll to reduce the number of SPI transactions. + static constexpr int MOTOR_FAULT_CHECK_INTERVAL = 5; + + // SPI Chip Selects + static constexpr uint8_t FRONT_LEFT_MOTOR_CHIP_SELECT = 0; + static constexpr uint8_t BACK_LEFT_MOTOR_CHIP_SELECT = 1; + static constexpr uint8_t BACK_RIGHT_MOTOR_CHIP_SELECT = 2; + static constexpr uint8_t FRONT_RIGHT_MOTOR_CHIP_SELECT = 3; + static constexpr uint8_t DRIBBLER_MOTOR_CHIP_SELECT = 4; + + // SPI Trinamic Motor Driver Paths + static const inline std::unordered_map SPI_PATHS = { + {MotorIndex::FRONT_LEFT, "/dev/spidev0.0"}, + {MotorIndex::FRONT_RIGHT, "/dev/spidev0.3"}, + {MotorIndex::BACK_LEFT, "/dev/spidev0.1"}, + {MotorIndex::BACK_RIGHT, "/dev/spidev0.2"}, + {MotorIndex::DRIBBLER, "/dev/spidev0.4"}, + }; + + static const inline std::unordered_map CHIP_SELECTS = { + {MotorIndex::FRONT_LEFT, FRONT_LEFT_MOTOR_CHIP_SELECT}, + {MotorIndex::FRONT_RIGHT, FRONT_RIGHT_MOTOR_CHIP_SELECT}, + {MotorIndex::BACK_LEFT, BACK_LEFT_MOTOR_CHIP_SELECT}, + {MotorIndex::BACK_RIGHT, BACK_RIGHT_MOTOR_CHIP_SELECT}, + {MotorIndex::DRIBBLER, DRIBBLER_MOTOR_CHIP_SELECT}, + }; + + // SPI Configs + static constexpr uint32_t TMC6100_SPI_SPEED = 1000000; // 1 Mhz + static constexpr uint32_t TMC4671_SPI_SPEED = 1000000; // 1 Mhz + static constexpr uint32_t MAX_SPI_SPEED_HZ = 2000000; // 2 Mhz + static constexpr uint8_t SPI_BITS = 8; + static constexpr uint32_t SPI_MODE = 0x3u; + + // SPI File Descriptors mapping from Chip Select -> File Descriptor + std::array()> file_descriptors_; + + // Number of times that Thunderloop will try to write the configuration to the driver + // before giving up + static constexpr int NUM_RETRIES_SPI = 3; + + // Trinamics communicate with 5 byte messages + static constexpr uint32_t TMC_CMD_MSG_SIZE = 5; + + static constexpr int DRIVE_MOTOR_NUM_POLE_PAIRS = 8; + static constexpr int DRIBBLER_MOTOR_NUM_POLE_PAIRS = 1; + + static constexpr double WHEEL_ROTATIONS_PER_MOTOR_ROTATION = 17.0 / 60.0; + + // All Trinamic RPMs are Electrical RPMs (eRPM), which represents the speed at which + // the rotating magnetic field inside the motor moves (as opposed to mechanical RPM + // which indicates how fast the motor shaft is rotating). + // + // RPM = eRPM / # of pole pairs + static constexpr double DRIVE_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM = + 1.0 / DRIVE_MOTOR_NUM_POLE_PAIRS * WHEEL_ROTATIONS_PER_MOTOR_ROTATION; + static constexpr double DRIBBLER_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM = + 1.0 / DRIBBLER_MOTOR_NUM_POLE_PAIRS; + static constexpr double DRIVE_MOTOR_ELECTRICAL_RPM_PER_MECHANICAL_RPM = + 1.0 / DRIVE_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM; + static constexpr double DRIBBLER_MOTOR_ELECTRICAL_RPM_PER_MECHANICAL_RPM = + 1.0 / DRIBBLER_MOTOR_MECHANICAL_RPM_PER_ELECTRICAL_RPM; + + static constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO = 16; + static constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO = 19; + static constexpr int MOTOR_DRIVER_RESET_GPIO = 12; + static constexpr int DRIVER_CONTROL_ENABLE_GPIO = 22; +}; \ No newline at end of file From a159d61181a9864a7257f4910eeaa1ef3518bb9e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Sat, 23 May 2026 00:12:21 +0000 Subject: [PATCH 12/23] [pre-commit.ci lite] apply automatic fixes --- src/extlibs/cppcrc.BUILD | 2 +- src/proto/robot_status_msg.proto | 2 +- src/software/embedded/gpio/BUILD | 2 +- src/software/embedded/gpio/gpio.cpp | 4 ++-- src/software/embedded/gpio/gpio.h | 1 + src/software/embedded/motor_controller/BUILD | 8 +++++++- src/software/embedded/motor_controller/motor_board.h | 2 +- .../embedded/motor_controller/motor_controller.h | 2 +- .../motor_controller/motor_fault_indicator.cpp | 2 +- .../motor_controller/motor_fault_indicator.h | 2 +- .../motor_controller/stspin_motor_controller.cpp | 12 ++++++++---- .../motor_controller/stspin_motor_controller.h | 4 ++-- .../stspin_motor_controller_test.cpp | 2 +- .../embedded/motor_controller/stspin_types.h | 2 +- .../motor_controller/tmc_motor_controller.cpp | 3 +-- .../embedded/motor_controller/tmc_motor_controller.h | 2 +- src/software/embedded/services/BUILD | 2 +- src/software/embedded/services/motor.cpp | 8 +++++--- src/software/embedded/services/motor.h | 2 +- src/software/embedded/spi_utils.cpp | 2 +- src/software/embedded/spi_utils.h | 7 ++++--- 21 files changed, 43 insertions(+), 30 deletions(-) diff --git a/src/extlibs/cppcrc.BUILD b/src/extlibs/cppcrc.BUILD index 80a1586127..264d46bf5e 100644 --- a/src/extlibs/cppcrc.BUILD +++ b/src/extlibs/cppcrc.BUILD @@ -6,4 +6,4 @@ cc_library( name = "cppcrc", hdrs = ["cppcrc.h"], visibility = ["//visibility:public"], -) \ No newline at end of file +) diff --git a/src/proto/robot_status_msg.proto b/src/proto/robot_status_msg.proto index a326a6deec..f645bfae21 100644 --- a/src/proto/robot_status_msg.proto +++ b/src/proto/robot_status_msg.proto @@ -101,7 +101,7 @@ enum MotorFault PHASE_W_SHORT_TO_GND_DETECTED = 13; PHASE_W_SHORT_TO_VS_DETECTED = 14; - /*********** STSPIN Faults ************/ + /*********** STSPIN Faults ************/ NO_FAULT = 15; DURATION = 16; OVER_VOLT = 17; diff --git a/src/software/embedded/gpio/BUILD b/src/software/embedded/gpio/BUILD index 6b79ab53d6..a09031ccf2 100644 --- a/src/software/embedded/gpio/BUILD +++ b/src/software/embedded/gpio/BUILD @@ -15,4 +15,4 @@ cc_library( "//software/logger:network_logger", "//software/util/make_enum", ], -) \ No newline at end of file +) diff --git a/src/software/embedded/gpio/gpio.cpp b/src/software/embedded/gpio/gpio.cpp index 7b609fd349..e01d22be33 100644 --- a/src/software/embedded/gpio/gpio.cpp +++ b/src/software/embedded/gpio/gpio.cpp @@ -10,7 +10,7 @@ bool Gpio::pollValue(GpioState state, std::chrono::milliseconds timeout_ms) { std::this_thread::sleep_for(std::chrono::microseconds(100)); - const auto current_time = std::chrono::system_clock::now(); // Use steady clock + const auto current_time = std::chrono::system_clock::now(); // Use steady clock const auto elapsed_time = current_time - start_time; if (elapsed_time > timeout_ms) { @@ -18,4 +18,4 @@ bool Gpio::pollValue(GpioState state, std::chrono::milliseconds timeout_ms) } } return true; -} \ No newline at end of file +} diff --git a/src/software/embedded/gpio/gpio.h b/src/software/embedded/gpio/gpio.h index ae5e6c9ac1..da60b19c50 100644 --- a/src/software/embedded/gpio/gpio.h +++ b/src/software/embedded/gpio/gpio.h @@ -1,6 +1,7 @@ #pragma once #include + #include "software/util/make_enum/make_enum.hpp" MAKE_ENUM(GpioState, LOW, HIGH); diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index ffe0cc3997..26518edbfd 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -1,4 +1,5 @@ package(default_visibility = ["//visibility:public"]) + cc_library( name = "motor_index", hdrs = ["motor_index.h"], @@ -6,6 +7,7 @@ cc_library( "//software/util/make_enum", ], ) + cc_library( name = "motor_board", hdrs = ["motor_board.h"], @@ -13,6 +15,7 @@ cc_library( "//software/util/make_enum", ], ) + cc_library( name = "motor_controller", srcs = [ @@ -38,6 +41,7 @@ cc_library( "@trinamic", ], ) + cc_library( name = "motor_fault_indicator", srcs = ["motor_fault_indicator.cpp"], @@ -47,10 +51,12 @@ cc_library( "//proto:tbots_cc_proto", ], ) + cc_library( name = "stspin_types", hdrs = ["stspin_types.h"], ) + cc_binary( name = "stspin_motor_controller_test", srcs = ["stspin_motor_controller_test.cpp"], @@ -64,4 +70,4 @@ cc_binary( "//software/logger:network_logger", "@boost//:program_options", ], -) \ No newline at end of file +) diff --git a/src/software/embedded/motor_controller/motor_board.h b/src/software/embedded/motor_controller/motor_board.h index 5a8bd32bb7..5076a78064 100644 --- a/src/software/embedded/motor_controller/motor_board.h +++ b/src/software/embedded/motor_controller/motor_board.h @@ -8,4 +8,4 @@ MAKE_ENUM(MotorBoard, TRINAMIC, STSPIN); constexpr MotorBoard MOTOR_BOARD = MotorBoard::TRINAMIC; #elif STSPIN_MOTOR_BOARD constexpr MotorBoard MOTOR_BOARD = MotorBoard::STSPIN; -#endif \ No newline at end of file +#endif diff --git a/src/software/embedded/motor_controller/motor_controller.h b/src/software/embedded/motor_controller/motor_controller.h index ee4bdbec87..5682a2791e 100644 --- a/src/software/embedded/motor_controller/motor_controller.h +++ b/src/software/embedded/motor_controller/motor_controller.h @@ -54,4 +54,4 @@ class MotorController * Immediately disables all motors. */ virtual void immediatelyDisable() = 0; -}; \ No newline at end of file +}; diff --git a/src/software/embedded/motor_controller/motor_fault_indicator.cpp b/src/software/embedded/motor_controller/motor_fault_indicator.cpp index 514a1e7606..6245ce2c43 100644 --- a/src/software/embedded/motor_controller/motor_fault_indicator.cpp +++ b/src/software/embedded/motor_controller/motor_fault_indicator.cpp @@ -11,4 +11,4 @@ MotorFaultIndicator::MotorFaultIndicator( bool MotorFaultIndicator::requiresReset() const { return !drive_enabled || faults.find(TbotsProto::MotorFault::RESET) != faults.end(); -} \ No newline at end of file +} diff --git a/src/software/embedded/motor_controller/motor_fault_indicator.h b/src/software/embedded/motor_controller/motor_fault_indicator.h index aacfe9718f..9e5419b787 100644 --- a/src/software/embedded/motor_controller/motor_fault_indicator.h +++ b/src/software/embedded/motor_controller/motor_fault_indicator.h @@ -35,4 +35,4 @@ struct MotorFaultIndicator * @return true if the motor requires a reset, false otherwise */ bool requiresReset() const; -}; \ No newline at end of file +}; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 5836cf42f1..5a34acdbaf 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -164,8 +164,12 @@ void StSpinMotorController::updateFaults(const MotorIndex motor, motor_faults.drive_enabled = false; } - LOG(WARNING) << oss.str(); //Consider moving this to a helper and also making the stream object persistent so we do not need to continuously recreate it (it is notoriously expensive). We could also consider another approach to formating this string as using streams in general is less than ideal. Or We can probably just use string::append - + LOG(WARNING) + << oss.str(); // Consider moving this to a helper and also making the stream + // object persistent so we do not need to continuously recreate it + // (it is notoriously expensive). We could also consider another + // approach to formatting this string as using streams in general + // is less than ideal. Or We can probably just use string::append } int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, @@ -192,7 +196,7 @@ void StSpinMotorController::updateEuclideanVelocity( const Vector local_velocity(target_euclidean_velocity[1], -target_euclidean_velocity[0]); - if (local_velocity.length() <= 0.01) //Magic number + if (local_velocity.length() <= 0.01) // Magic number { sendAndReceiveFrame( MotorIndex::FRONT_LEFT, @@ -460,4 +464,4 @@ void StSpinMotorController::sendMotorStatusToPlotJuggler(const MotorIndex motor) {"id_" + motor, motor_status.id}, {"id_ref_" + motor, motor_status.id_ref}, }); -} \ No newline at end of file +} diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 68f0c26092..f3cb911c64 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -62,7 +62,7 @@ class StSpinMotorController : public MotorController static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 750; static constexpr int MIN_SPEED_FEED_FORWARD_STATIC_GAIN = 300; - robot_constants::RobotConstants robot_constants_; + robot_constants::RobotConstants robot_constants_; // SPI file descriptors std::unordered_map spi_fds_; @@ -139,4 +139,4 @@ class StSpinMotorController : public MotorController void sendMotorStatusToPlotJuggler(MotorIndex motor); friend class StSpinMotorControllerTest; -}; \ No newline at end of file +}; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp index 80b657f781..1f54b929f5 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -279,4 +279,4 @@ int main(int argc, char** argv) stspin_motor_controller_test.runMotorTest(); return 0; -} \ No newline at end of file +} diff --git a/src/software/embedded/motor_controller/stspin_types.h b/src/software/embedded/motor_controller/stspin_types.h index 82417386df..7b1ba2e3c8 100644 --- a/src/software/embedded/motor_controller/stspin_types.h +++ b/src/software/embedded/motor_controller/stspin_types.h @@ -90,4 +90,4 @@ struct SetSpeedFeedForwardKaKvFrame struct SetSpeedFeedForwardKsFrame { int16_t ks; -}; \ No newline at end of file +}; diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.cpp b/src/software/embedded/motor_controller/tmc_motor_controller.cpp index a917e6284f..468919fd4c 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.cpp +++ b/src/software/embedded/motor_controller/tmc_motor_controller.cpp @@ -1,6 +1,5 @@ #include "software/embedded/motor_controller/tmc_motor_controller.h" - #include "shared/constants.h" #include "software/embedded/gpio/gpio_char_dev.h" #include "software/embedded/spi_utils.h" @@ -746,4 +745,4 @@ void TmcMotorController::endEncoderCalibration(const MotorIndex motor) void TmcMotorController::immediatelyDisable() { driver_control_enable_gpio_->setValue(GpioState::LOW); -} \ No newline at end of file +} diff --git a/src/software/embedded/motor_controller/tmc_motor_controller.h b/src/software/embedded/motor_controller/tmc_motor_controller.h index 8cb3c0889b..71a8a090f3 100644 --- a/src/software/embedded/motor_controller/tmc_motor_controller.h +++ b/src/software/embedded/motor_controller/tmc_motor_controller.h @@ -294,4 +294,4 @@ class TmcMotorController : public MotorController static constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO = 19; static constexpr int MOTOR_DRIVER_RESET_GPIO = 12; static constexpr int DRIVER_CONTROL_ENABLE_GPIO = 22; -}; \ No newline at end of file +}; diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index dda1bccd1e..528b1eedc9 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -35,4 +35,4 @@ cc_library( "@boost//:asio", "@boost//:filesystem", ], -) \ No newline at end of file +) diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index f5f89e1447..6c838b0c5a 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -10,7 +10,7 @@ MotorService::MotorService(const robot_constants::RobotConstants& robot_constant : robot_constants_(robot_constants), motor_controller_(setupMotorController()), euclidean_to_four_wheel_(robot_constants), - dribbler_target_rpm_(0), //placeholder, move to power_service + dribbler_target_rpm_(0), // placeholder, move to power_service drive_motor_mps_per_rpm_(2 * M_PI * robot_constants.wheel_radius_meters / 60), num_tracked_motor_resets_(0) { @@ -34,7 +34,9 @@ std::unique_ptr MotorService::setupMotorController() if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) { return std::make_unique(); - } else { + } + else + { return std::make_unique(robot_constants_); } } @@ -289,4 +291,4 @@ bool MotorService::anyMotorRequiresReset() const reflective_enum::values().end(), [&](const MotorIndex motor) { return motor_controller_->checkFaults(motor).requiresReset(); }); -} \ No newline at end of file +} diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index c05355e695..445fb03ff5 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -104,4 +104,4 @@ class MotorService static constexpr int MOTOR_RESET_THRESHOLD_COUNT = 3; static constexpr double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; static constexpr int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; -}; \ No newline at end of file +}; diff --git a/src/software/embedded/spi_utils.cpp b/src/software/embedded/spi_utils.cpp index 0962d6e4ee..207f02e90b 100644 --- a/src/software/embedded/spi_utils.cpp +++ b/src/software/embedded/spi_utils.cpp @@ -56,4 +56,4 @@ void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, const uint8_t* wri CHECK(ret1 >= 1 && ret2 >= 1) << "SPI Transfer to motor failed, not safe to proceed: errno " << strerror(errno); -} \ No newline at end of file +} diff --git a/src/software/embedded/spi_utils.h b/src/software/embedded/spi_utils.h index 30aaea4f67..c7b170d56a 100644 --- a/src/software/embedded/spi_utils.h +++ b/src/software/embedded/spi_utils.h @@ -31,6 +31,7 @@ void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, * @param read_rx the buffer our read response will be placed in * @param spi_speed the speed to run spi at in Hz */ -void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, const uint8_t* write_tx, - const uint8_t* read_rx, const uint32_t read_len, - const uint32_t write_len, uint32_t spi_speed); //refactor to take std::array, not raw pointers \ No newline at end of file +void readThenWriteSpiTransfer( + int fd, const uint8_t* read_tx, const uint8_t* write_tx, const uint8_t* read_rx, + const uint32_t read_len, const uint32_t write_len, + uint32_t spi_speed); // refactor to take std::array, not raw pointers From bb45e94998aa0c753b69ac2bc014b38948ed377f Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 19:31:47 -0700 Subject: [PATCH 13/23] embedded BUILD file --- src/software/embedded/BUILD | 50 ++++++++++++++++++++++++------------- 1 file changed, 32 insertions(+), 18 deletions(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index d28b7ff4eb..5ee03cb54c 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -2,6 +2,29 @@ load("@bazel_skylib//rules:common_settings.bzl", "string_flag") package(default_visibility = ["//visibility:public"]) +config_setting( + name = "build_trinamic", + flag_values = { + ":motor_board": "TRINAMIC", + }, +) + +config_setting( + name = "build_stspin", + flag_values = { + ":motor_board": "STSPIN", + }, +) + +string_flag( + name = "motor_board", + build_setting_default = "TRINAMIC", + values = [ + "TRINAMIC", + "STSPIN", + ], +) + cc_library( name = "primitive_executor", srcs = ["primitive_executor.cpp"], @@ -19,6 +42,15 @@ cc_library( ], ) +cc_library( + name = "spi_utils", + srcs = ["spi_utils.cpp"], + hdrs = ["spi_utils.h"], + deps = [ + "//software/logger", + ], +) + cc_library( name = "thunderloop", srcs = ["thunderloop.cpp"], @@ -37,24 +69,6 @@ cc_library( ], ) -cc_library( - name = "gpio", - srcs = [ - "gpio_char_dev.cpp", - "gpio_sysfs.cpp", - ], - hdrs = [ - "gpio.h", - "gpio_char_dev.h", - "gpio_sysfs.h", - ], - deps = [ - "//software/logger", - "//software/logger:network_logger", - "//software/util/make_enum", - ], -) - cc_binary( name = "thunderloop_main", srcs = ["thunderloop_main.cpp"], From 22870e84d5cd36993838f9014215223719b6261d Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 20:18:10 -0700 Subject: [PATCH 14/23] fix ansible BUILD --- src/software/embedded/ansible/BUILD | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/embedded/ansible/BUILD b/src/software/embedded/ansible/BUILD index 1225195e86..2417b4b99e 100644 --- a/src/software/embedded/ansible/BUILD +++ b/src/software/embedded/ansible/BUILD @@ -23,8 +23,8 @@ py_binary( "//software/embedded:thunderloop_main", "//software/embedded/linux_configs/pi:pi_files", "//software/embedded/linux_configs/systemd:systemd_files", + "//software/embedded/motor_controller:stspin_motor_controller_test", "//software/embedded/robot_diagnostics_cli:robot_diagnostics_cli_tar", - "//software/embedded/services:robot_auto_test", "//software/power:powerloop_tar", ], deps = [ From fac0ace7cc55b6b30371bae265afaab7a4039d4f Mon Sep 17 00:00:00 2001 From: Grayson Date: Sat, 23 May 2026 13:06:27 -0700 Subject: [PATCH 15/23] docs and motor_index --- docs/useful-robot-commands.md | 13 ++++++++----- .../embedded/motor_controller/motor_index.h | 17 +++++++++++++++++ 2 files changed, 25 insertions(+), 5 deletions(-) create mode 100644 src/software/embedded/motor_controller/motor_index.h diff --git a/docs/useful-robot-commands.md b/docs/useful-robot-commands.md index 3698408105..d6f6d2bdf0 100644 --- a/docs/useful-robot-commands.md +++ b/docs/useful-robot-commands.md @@ -4,6 +4,7 @@ +- [Useful Robot Commands](#useful-robot-commands) - [Table of Contents](#table-of-contents) - [Flashing the robot's compute module](#flashing-the-robots-compute-module) - [Flashing the powerboard](#flashing-the-powerboard) @@ -12,7 +13,7 @@ - [Robot Diagnostics](#robot-diagnostics) - [For Just Diagnostics](#for-just-diagnostics) - [For AI + Diagnostics](#for-ai--diagnostics) - - [Robot Auto Test](#robot-auto-test) + - [STSPIN Motor Controller Test](#stspin-motor-controller-test) - [On Robot Commands](#on-robot-commands) - [Systemd Services](#systemd-services) - [Debugging Uart](#debugging-uart) @@ -76,9 +77,11 @@ This will stop the current Systemd services, replace and restart them. Binaries This will trigger motor calibration meaning the wheels may spin. Please elevate the robot so the wheels are not touching the ground for proper calibration. ```bash -bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot -- --playbook deploy_robot_software.yml --hosts --ssh_pass +bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot --//software/embedded:motor_board= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ``` +* is the type of motor driver board on the robot (either `STSPIN` or `TRINAMIC`) + * is the IP address of the robot * is the password of the `robot` user account @@ -140,13 +143,13 @@ From Software/src network_interface can be found with `ifconfig` commonly `wlp59s0` for wifi. -## Robot Auto Test -Runs the robot auto test fixture on a robot through Ansible, which tests the motor board and power board SPI and UART transfer respectively. +## STSPIN Motor Controller Test +Deploys the STSPIN Motor Controller Test binary onto a robot through Ansible. From Software/src: ```bash -bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot -- --playbook robot_auto_test_playbook.yml --hosts --ssh_pass +bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:robot --//software/embedded:motor_board=STSPIN -- --playbook deploy_stspin_motor_controller_test.yml --hosts --ssh_pass ``` * replace the \ with the actual ip address of the Raspberry Pi for the ssh connection. diff --git a/src/software/embedded/motor_controller/motor_index.h b/src/software/embedded/motor_controller/motor_index.h new file mode 100644 index 0000000000..70536a2ba8 --- /dev/null +++ b/src/software/embedded/motor_controller/motor_index.h @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include "software/util/make_enum/make_enum.hpp" + +MAKE_ENUM(MotorIndex, FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT, DRIBBLER); + +constexpr std::array driveMotors() +{ + return { + MotorIndex::FRONT_LEFT, + MotorIndex::FRONT_RIGHT, + MotorIndex::BACK_LEFT, + MotorIndex::BACK_RIGHT, + }; +} \ No newline at end of file From fe4ce55ebf9f85a1feb3e8ea5a08a3dc2dd5f2d3 Mon Sep 17 00:00:00 2001 From: Grayson Date: Sat, 23 May 2026 14:37:50 -0700 Subject: [PATCH 16/23] various fixes --- docs/useful-robot-commands.md | 1 - src/proto/robot_status_msg.proto | 16 +++-- .../deploy_stspin_motor_controller_test.yml | 42 ++++++++++++ .../playbooks/robot_auto_test_playbook.yml | 66 ------------------- src/software/embedded/constants/BUILD | 7 -- src/software/embedded/constants/constants.h | 11 ---- src/software/embedded/thunderloop.h | 3 + 7 files changed, 54 insertions(+), 92 deletions(-) create mode 100644 src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml delete mode 100644 src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml delete mode 100644 src/software/embedded/constants/constants.h diff --git a/docs/useful-robot-commands.md b/docs/useful-robot-commands.md index d6f6d2bdf0..eb6862c3f8 100644 --- a/docs/useful-robot-commands.md +++ b/docs/useful-robot-commands.md @@ -81,7 +81,6 @@ bazel run //software/embedded/ansible:run_ansible --platforms=//toolchains/cc:ro ``` * is the type of motor driver board on the robot (either `STSPIN` or `TRINAMIC`) - * is the IP address of the robot * is the password of the `robot` user account diff --git a/src/proto/robot_status_msg.proto b/src/proto/robot_status_msg.proto index f645bfae21..3277ebe515 100644 --- a/src/proto/robot_status_msg.proto +++ b/src/proto/robot_status_msg.proto @@ -128,13 +128,15 @@ message DriveUnit /* Data from all four drive units and the dribbler */ message MotorStatus { - DriveUnit front_left = 1; - DriveUnit front_right = 2; - DriveUnit back_left = 3; - DriveUnit back_right = 4; - DribblerStatus dribbler = 5; - Vector local_velocity = 6; - AngularVelocity angular_velocity = 7; + DriveUnit front_left = 1; + DriveUnit front_right = 2; + DriveUnit back_left = 3; + DriveUnit back_right = 4; + DribblerStatus dribbler = 5; + Vector local_velocity = 6; + AngularVelocity angular_velocity = 7; + Vector target_local_velocity = 8; + AngularVelocity target_angular_velocity = 9; } /* Data about the network connection with the robots, including network-derived values */ diff --git a/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml b/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml new file mode 100644 index 0000000000..6f9e992f64 --- /dev/null +++ b/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml @@ -0,0 +1,42 @@ +--- +- name: Deploy stspin_motor_controller_test binary to robot + hosts: THUNDERBOTS_HOSTS + + tasks: + - name: Log Robot ID + ansible.builtin.debug: + msg: "[Robot ID = {{ inventory_hostname }}]" + tags: always + + - name: Deploy stspin_motor_controller_test binary + block: + - name: Stop Services + become: true + become_method: ansible.builtin.sudo + ansible.builtin.systemd: + name: "thunderloop.service" + masked: false + daemon_reload: true + state: stopped + tags: always + + - name: Delete stspin_motor_controller_test binary under ~/thunderbots_binaries + ansible.builtin.file: + state: absent + path: ~/thunderbots_binaries/stspin_motor_controller_test + become_method: ansible.builtin.sudo + become: true + register: result + + - name: Log where Ansible is searching + ansible.builtin.debug: + msg: "{{ ansible_search_path }}" + tags: always + + - name: Sync Binary + ansible.posix.synchronize: + src: ../../motor_controller/stspin_motor_controller_test + dest: ~/thunderbots_binaries/ + recursive: true + copy_links: true + tags: always \ No newline at end of file diff --git a/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml b/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml deleted file mode 100644 index 5e337bacf4..0000000000 --- a/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml +++ /dev/null @@ -1,66 +0,0 @@ ---- -- name: Robot Auto Test Playbook - hosts: THUNDERBOTS_HOSTS - - tasks: - - name: Log Robot ID - ansible.builtin.debug: - msg: "[Robot ID = {{ inventory_hostname }}]" - tags: always - - - name: Start Robot Auto Test Process - block: - - name: Stop Services - become: true - become_method: ansible.builtin.sudo - ansible.builtin.systemd: - name: "thunderloop.service" - masked: false - daemon_reload: true - state: stopped - tags: always - - - name: Delete robot_auto_test binary under ~/thunderbots_binaries - ansible.builtin.file: - state: absent - path: ~/thunderbots_binaries/robot_auto_test - become_method: ansible.builtin.sudo - become: true - register: result - - - name: Log where Ansible is searching - ansible.builtin.debug: - msg: "{{ ansible_search_path }}" - tags: always - - - name: Sync Binary - ansible.posix.synchronize: - src: ../../services/robot_auto_test - dest: ~/thunderbots_binaries/ - recursive: true - copy_links: true - tags: always - - - name: Run Binary - ansible.builtin.command: "./robot_auto_test" - args: - chdir: /home/robot/thunderbots_binaries - become_method: ansible.builtin.sudo - become: true - register: robot_auto_test_output - changed_when: true - ignore_errors: true - - - name: Print Binary Output - ansible.builtin.debug: - var: robot_auto_test_output.stdout - - - name: Start Services - become: true - become_method: ansible.builtin.sudo - ansible.builtin.systemd: - name: "thunderloop.service" - masked: false - daemon_reload: true - state: started - tags: always diff --git a/src/software/embedded/constants/BUILD b/src/software/embedded/constants/BUILD index 1d6c07337c..1f3467963e 100644 --- a/src/software/embedded/constants/BUILD +++ b/src/software/embedded/constants/BUILD @@ -1,12 +1,5 @@ package(default_visibility = ["//visibility:public"]) -cc_library( - name = "constants", - hdrs = [ - "constants.h", - ], -) - py_library( name = "py_constants", srcs = [":py_constants.py"], diff --git a/src/software/embedded/constants/constants.h b/src/software/embedded/constants/constants.h deleted file mode 100644 index 3a811a5d7c..0000000000 --- a/src/software/embedded/constants/constants.h +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -// Constants for the Raspberry Pi - -constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO = 16; -constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO = 19; -constexpr int MOTOR_DRIVER_RESET_GPIO = 12; -constexpr int DRIVER_CONTROL_ENABLE_GPIO = 22; - -// Path to the CPU thermal zone temperature file -constexpr const char CPU_TEMP_FILE_PATH[] = "/sys/class/thermal/thermal_zone0/temp"; diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index cf70a12b68..6d12cd4c0d 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -164,6 +164,9 @@ class Thunderloop const std::string PATH_TO_RINGBUFFER_LOG = "/usr/bin/dmesg"; std::ifstream log_file = std::ifstream(PATH_TO_RINGBUFFER_LOG); + + // Path to the CPU thermal zone temperature file + const std::string CPU_TEMP_FILE_PATH = "/sys/class/thermal/thermal_zone0/temp"; }; /* From 4f7df9449e50566a6cdb45d8224a631c6e5582fb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Sat, 23 May 2026 21:43:12 +0000 Subject: [PATCH 17/23] [pre-commit.ci lite] apply automatic fixes --- docs/useful-robot-commands.md | 1 - .../ansible/playbooks/deploy_stspin_motor_controller_test.yml | 2 +- src/software/embedded/motor_controller/motor_index.h | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/docs/useful-robot-commands.md b/docs/useful-robot-commands.md index eb6862c3f8..1aced54d42 100644 --- a/docs/useful-robot-commands.md +++ b/docs/useful-robot-commands.md @@ -4,7 +4,6 @@ -- [Useful Robot Commands](#useful-robot-commands) - [Table of Contents](#table-of-contents) - [Flashing the robot's compute module](#flashing-the-robots-compute-module) - [Flashing the powerboard](#flashing-the-powerboard) diff --git a/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml b/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml index 6f9e992f64..54676dd0fd 100644 --- a/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml +++ b/src/software/embedded/ansible/playbooks/deploy_stspin_motor_controller_test.yml @@ -39,4 +39,4 @@ dest: ~/thunderbots_binaries/ recursive: true copy_links: true - tags: always \ No newline at end of file + tags: always diff --git a/src/software/embedded/motor_controller/motor_index.h b/src/software/embedded/motor_controller/motor_index.h index 70536a2ba8..4a58a63d6a 100644 --- a/src/software/embedded/motor_controller/motor_index.h +++ b/src/software/embedded/motor_controller/motor_index.h @@ -14,4 +14,4 @@ constexpr std::array driveMotors() MotorIndex::BACK_LEFT, MotorIndex::BACK_RIGHT, }; -} \ No newline at end of file +} From 12a8d46ffd268fad1ff3c7e0dc93e5174f51538f Mon Sep 17 00:00:00 2001 From: Grayson Date: Wed, 27 May 2026 23:05:03 -0700 Subject: [PATCH 18/23] address comments --- src/software/embedded/gpio/gpio.cpp | 4 ++-- src/software/embedded/services/BUILD | 1 + src/software/embedded/thunderloop.h | 3 --- 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/software/embedded/gpio/gpio.cpp b/src/software/embedded/gpio/gpio.cpp index e01d22be33..8a3cc8229c 100644 --- a/src/software/embedded/gpio/gpio.cpp +++ b/src/software/embedded/gpio/gpio.cpp @@ -5,12 +5,12 @@ bool Gpio::pollValue(GpioState state, std::chrono::milliseconds timeout_ms) { - const auto start_time = std::chrono::system_clock::now(); + const auto start_time = std::chrono::steady_clock::now(); while (getValue() != state) { std::this_thread::sleep_for(std::chrono::microseconds(100)); - const auto current_time = std::chrono::system_clock::now(); // Use steady clock + const auto current_time = std::chrono::steady_clock::now(); const auto elapsed_time = current_time - start_time; if (elapsed_time > timeout_ms) { diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index 528b1eedc9..227b8712af 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -8,6 +8,7 @@ cc_library( select({ "//software/embedded:build_trinamic": ["TRINAMIC_MOTOR_BOARD"], "//software/embedded:build_stspin": ["STSPIN_MOTOR_BOARD"], + "//conditions:default": ["STSPIN_MOTOR_BOARD"], }), deps = [ "//proto:tbots_cc_proto", diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 6d12cd4c0d..cf70a12b68 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -164,9 +164,6 @@ class Thunderloop const std::string PATH_TO_RINGBUFFER_LOG = "/usr/bin/dmesg"; std::ifstream log_file = std::ifstream(PATH_TO_RINGBUFFER_LOG); - - // Path to the CPU thermal zone temperature file - const std::string CPU_TEMP_FILE_PATH = "/sys/class/thermal/thermal_zone0/temp"; }; /* From 04e180844aac41fa0ebbd11d662cf00db2ab36dc Mon Sep 17 00:00:00 2001 From: Grayson Date: Wed, 27 May 2026 23:13:46 -0700 Subject: [PATCH 19/23] merge conflict --- src/software/embedded/services/BUILD | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index ecdd6131e3..fa9571a16c 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -37,8 +37,6 @@ cc_library( "@boost//:filesystem", ], ) -<<<<<<< HEAD -======= cc_library( name = "imu", @@ -53,23 +51,3 @@ cc_library( "@eigen", ], ) - -cc_binary( - name = "robot_auto_test", - srcs = ["robot_auto_test.cpp"], - linkopts = [ - "-lpthread", - "-lrt", - ], - deps = [ - ":motor", - ":power", - "//proto/message_translation:tbots_geometry", - "//proto/primitive:primitive_msg_factory", - "//shared:robot_constants", - "//software/embedded:primitive_executor", - "//software/logger", - "@trinamic", - ], -) ->>>>>>> 2a39569e785bb4f0f81ad646828e00942be3cfc2 From 5b6fd1304132578f3052256f81c27b183beb962a Mon Sep 17 00:00:00 2001 From: Grayson Date: Wed, 27 May 2026 23:19:13 -0700 Subject: [PATCH 20/23] re-add constants --- src/software/embedded/thunderloop.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index a7404f8b65..94ffee0693 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -166,6 +166,9 @@ class Thunderloop const std::string PATH_TO_RINGBUFFER_LOG = "/usr/bin/dmesg"; std::ifstream log_file = std::ifstream(PATH_TO_RINGBUFFER_LOG); + + // Path to the CPU thermal zone temperature file + const std::string CPU_TEMP_FILE_PATH = "/sys/class/thermal/thermal_zone0/temp"; }; /* From bd217170e406400d84765baf29163108267138c5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Thu, 28 May 2026 06:25:19 +0000 Subject: [PATCH 21/23] [pre-commit.ci lite] apply automatic fixes --- src/software/embedded/thunderloop.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 94ffee0693..94d43da359 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -166,7 +166,7 @@ class Thunderloop const std::string PATH_TO_RINGBUFFER_LOG = "/usr/bin/dmesg"; std::ifstream log_file = std::ifstream(PATH_TO_RINGBUFFER_LOG); - + // Path to the CPU thermal zone temperature file const std::string CPU_TEMP_FILE_PATH = "/sys/class/thermal/thermal_zone0/temp"; }; From 294a02841524aaa7bd1003b60377e83d79206b83 Mon Sep 17 00:00:00 2001 From: Grayson Date: Thu, 28 May 2026 16:39:01 -0700 Subject: [PATCH 22/23] link comments to issues --- src/proto/robot_status_msg.proto | 1 + .../embedded/motor_controller/stspin_motor_controller.cpp | 5 +++-- .../embedded/motor_controller/stspin_motor_controller.h | 3 +++ src/software/embedded/spi_utils.h | 3 ++- 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/proto/robot_status_msg.proto b/src/proto/robot_status_msg.proto index 3277ebe515..372aca6e51 100644 --- a/src/proto/robot_status_msg.proto +++ b/src/proto/robot_status_msg.proto @@ -101,6 +101,7 @@ enum MotorFault PHASE_W_SHORT_TO_GND_DETECTED = 13; PHASE_W_SHORT_TO_VS_DETECTED = 14; + // TODO: #3749 Move this to its own proto. /*********** STSPIN Faults ************/ NO_FAULT = 15; DURATION = 16; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 5a34acdbaf..2611fe066d 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -84,7 +84,8 @@ void StSpinMotorController::updateFaults(const MotorIndex motor, // No faults return; } - + + // TODO #3748 Use a helper, stop regenerating the stream object. std::ostringstream oss; oss << "======= Faults For Motor " << motor << "=======\n"; @@ -196,7 +197,7 @@ void StSpinMotorController::updateEuclideanVelocity( const Vector local_velocity(target_euclidean_velocity[1], -target_euclidean_velocity[0]); - if (local_velocity.length() <= 0.01) // Magic number + if (local_velocity.length() <= MINIMUM_SPEED_FOR_FEED_FORWARD) { sendAndReceiveFrame( MotorIndex::FRONT_LEFT, diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index f3cb911c64..15edf97cd4 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -32,6 +32,7 @@ class StSpinMotorController : public MotorController void immediatelyDisable() override; private: + // TODO: #3750 Use a template function instead of std::variant. using OutgoingFrame = std::variant -// Wrap in spi_utils namespace +// TODO: #3747 Wrap in spi_utils namespace +// TODO: #3751 Use std::array instead of raw pointers for tx and rx buffers. /** * Trigger an SPI transfer over an open SPI connection * From 8aaa52512e3fd84abb8acd072f876bc8ddba1af0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Thu, 28 May 2026 23:46:20 +0000 Subject: [PATCH 23/23] [pre-commit.ci lite] apply automatic fixes --- .../embedded/motor_controller/stspin_motor_controller.cpp | 2 +- .../embedded/motor_controller/stspin_motor_controller.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 2611fe066d..1c29915b4d 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -84,7 +84,7 @@ void StSpinMotorController::updateFaults(const MotorIndex motor, // No faults return; } - + // TODO #3748 Use a helper, stop regenerating the stream object. std::ostringstream oss; oss << "======= Faults For Motor " << motor << "=======\n"; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 15edf97cd4..1a3c2209cb 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -32,7 +32,7 @@ class StSpinMotorController : public MotorController void immediatelyDisable() override; private: - // TODO: #3750 Use a template function instead of std::variant. + // TODO: #3750 Use a template function instead of std::variant. using OutgoingFrame = std::variant