From b1fe7f8f74f7ba0b5f73e128dd0af3bc96129cd0 Mon Sep 17 00:00:00 2001 From: Grayson Date: Tue, 12 May 2026 22:02:12 -0700 Subject: [PATCH 01/34] setup dribbler interface --- src/proto/power_frame_msg.proto | 10 ++++++++-- src/software/power/dribbler.cpp | 10 ++++++++++ src/software/power/dribbler.h | 20 ++++++++++++++++++++ src/software/power/pins.h | 3 +++ src/software/power/powerloop_main.cc | 13 +++++++++++-- 5 files changed, 52 insertions(+), 4 deletions(-) create mode 100644 src/software/power/dribbler.cpp create mode 100644 src/software/power/dribbler.h diff --git a/src/proto/power_frame_msg.proto b/src/proto/power_frame_msg.proto index 9c708bf083..2ef85cf2d7 100644 --- a/src/proto/power_frame_msg.proto +++ b/src/proto/power_frame_msg.proto @@ -32,13 +32,19 @@ message PowerPulseControl Geneva.Slot geneva_slot = 2; } +message DribblerControl +{ + uint8 dribbler_speed = 1; +} + message PowerFrame { uint32 length = 1; uint32 crc = 2; oneof power_msg { - PowerPulseControl power_control = 3; - PowerStatus power_status = 4; + PowerPulseControl power_control = 3; + PowerStatus power_status = 4; + DribblerControl dribbler_control = 5; } } diff --git a/src/software/power/dribbler.cpp b/src/software/power/dribbler.cpp new file mode 100644 index 0000000000..d82d37e1a1 --- /dev/null +++ b/src/software/power/dribbler.cpp @@ -0,0 +1,10 @@ +#include "dribbler.h" + +Dribbler::Dribbler() +{ + pinMode(DRIBBLER_PIN, OUTPUT); +} + +void Dribbler::dribble(uint32_t speed) { + analogWrite(DRIBBLER_PIN, speed); +} \ No newline at end of file diff --git a/src/software/power/dribbler.h b/src/software/power/dribbler.h new file mode 100644 index 0000000000..355e902104 --- /dev/null +++ b/src/software/power/dribbler.h @@ -0,0 +1,20 @@ +#pragma once + +#include + +/** + * Represents the dribbler on the power board + */ +class Dribbler +{ + public: + /** + * Creates a dribbler, setting up pins. + */ + Dribbler(); + + /** + * Sets the dribbler to the given duty cycle from 0 (always off) to 255 (always on) + */ + static void dribble(uint32_t speed); +} \ No newline at end of file diff --git a/src/software/power/pins.h b/src/software/power/pins.h index fdbdd9c20b..2d18caf001 100644 --- a/src/software/power/pins.h +++ b/src/software/power/pins.h @@ -31,6 +31,9 @@ const uint8_t TXD2 = 22; const uint8_t PM_SDA = 13; const uint8_t PM_SCL = 14; +// Dribbler +const uint8_t DRIBBLER_PIN = 0; // When elec has a pin for this + // Timers const uint8_t CHICKER_PULSE_TIMER = 0; const uint8_t CHICKER_COOLDOWN_TIMER = 3; diff --git a/src/software/power/powerloop_main.cc b/src/software/power/powerloop_main.cc index 692f70bed8..a32505d7bb 100644 --- a/src/software/power/powerloop_main.cc +++ b/src/software/power/powerloop_main.cc @@ -3,6 +3,7 @@ #include "chicker.h" #include "constants_platformio.h" #include "control_executor.h" +#include "dribbler.h" #include "geneva.h" #include "power_frame_msg_platformio.h" #include "power_monitor.h" @@ -34,6 +35,7 @@ std::shared_ptr chicker; std::shared_ptr monitor; std::shared_ptr geneva; std::shared_ptr executor; +std::shared_ptr dribbler; void setup() { @@ -45,6 +47,7 @@ void setup() monitor = std::make_shared(); geneva = std::make_shared(); executor = std::make_shared(charger, chicker, geneva); + dribbler = std::make_shared(); charger->chargeCapacitors(); } @@ -63,8 +66,14 @@ void loop() if (unmarshalUartPacket(buffer, frame)) { // On successful decoding execute the given command - TbotsProto_PowerPulseControl control = frame.power_msg.power_control; - executor->execute(control); + if (frame.which_power_msg == TbotsProto_PowerFrame_power_control_tag) + { + executor->execute(frame.power_msg.power_control); + } + else if (frame.which_power_msg == TbotsProto_PowerFrame_dribbler_control_tag) + { + dribbler->dribble(frame.power_msg.dribble_control.dribble_speed); + } } buffer.clear(); From 44ae4473d2fb287710403a3df4e3926c5197376b Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 22 May 2026 16:01:26 -0700 Subject: [PATCH 02/34] 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 03/34] 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 04/34] 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 05/34] 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 06/34] 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 07/34] 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 08/34] 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 09/34] 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 10/34] 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 11/34] 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 12/34] 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 13/34] [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 14/34] 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 15/34] 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 16/34] 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 17/34] 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 18/34] [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 9b7758952f5e43d1f80a505e64e0128a09719a25 Mon Sep 17 00:00:00 2001 From: Grayson Date: Tue, 12 May 2026 22:02:12 -0700 Subject: [PATCH 19/34] setup dribbler interface --- src/proto/power_frame_msg.proto | 10 ++++++++-- src/software/power/dribbler.cpp | 10 ++++++++++ src/software/power/dribbler.h | 20 ++++++++++++++++++++ src/software/power/pins.h | 3 +++ src/software/power/powerloop_main.cc | 13 +++++++++++-- 5 files changed, 52 insertions(+), 4 deletions(-) create mode 100644 src/software/power/dribbler.cpp create mode 100644 src/software/power/dribbler.h diff --git a/src/proto/power_frame_msg.proto b/src/proto/power_frame_msg.proto index 9c708bf083..2ef85cf2d7 100644 --- a/src/proto/power_frame_msg.proto +++ b/src/proto/power_frame_msg.proto @@ -32,13 +32,19 @@ message PowerPulseControl Geneva.Slot geneva_slot = 2; } +message DribblerControl +{ + uint8 dribbler_speed = 1; +} + message PowerFrame { uint32 length = 1; uint32 crc = 2; oneof power_msg { - PowerPulseControl power_control = 3; - PowerStatus power_status = 4; + PowerPulseControl power_control = 3; + PowerStatus power_status = 4; + DribblerControl dribbler_control = 5; } } diff --git a/src/software/power/dribbler.cpp b/src/software/power/dribbler.cpp new file mode 100644 index 0000000000..d82d37e1a1 --- /dev/null +++ b/src/software/power/dribbler.cpp @@ -0,0 +1,10 @@ +#include "dribbler.h" + +Dribbler::Dribbler() +{ + pinMode(DRIBBLER_PIN, OUTPUT); +} + +void Dribbler::dribble(uint32_t speed) { + analogWrite(DRIBBLER_PIN, speed); +} \ No newline at end of file diff --git a/src/software/power/dribbler.h b/src/software/power/dribbler.h new file mode 100644 index 0000000000..355e902104 --- /dev/null +++ b/src/software/power/dribbler.h @@ -0,0 +1,20 @@ +#pragma once + +#include + +/** + * Represents the dribbler on the power board + */ +class Dribbler +{ + public: + /** + * Creates a dribbler, setting up pins. + */ + Dribbler(); + + /** + * Sets the dribbler to the given duty cycle from 0 (always off) to 255 (always on) + */ + static void dribble(uint32_t speed); +} \ No newline at end of file diff --git a/src/software/power/pins.h b/src/software/power/pins.h index fdbdd9c20b..2d18caf001 100644 --- a/src/software/power/pins.h +++ b/src/software/power/pins.h @@ -31,6 +31,9 @@ const uint8_t TXD2 = 22; const uint8_t PM_SDA = 13; const uint8_t PM_SCL = 14; +// Dribbler +const uint8_t DRIBBLER_PIN = 0; // When elec has a pin for this + // Timers const uint8_t CHICKER_PULSE_TIMER = 0; const uint8_t CHICKER_COOLDOWN_TIMER = 3; diff --git a/src/software/power/powerloop_main.cc b/src/software/power/powerloop_main.cc index 692f70bed8..a32505d7bb 100644 --- a/src/software/power/powerloop_main.cc +++ b/src/software/power/powerloop_main.cc @@ -3,6 +3,7 @@ #include "chicker.h" #include "constants_platformio.h" #include "control_executor.h" +#include "dribbler.h" #include "geneva.h" #include "power_frame_msg_platformio.h" #include "power_monitor.h" @@ -34,6 +35,7 @@ std::shared_ptr chicker; std::shared_ptr monitor; std::shared_ptr geneva; std::shared_ptr executor; +std::shared_ptr dribbler; void setup() { @@ -45,6 +47,7 @@ void setup() monitor = std::make_shared(); geneva = std::make_shared(); executor = std::make_shared(charger, chicker, geneva); + dribbler = std::make_shared(); charger->chargeCapacitors(); } @@ -63,8 +66,14 @@ void loop() if (unmarshalUartPacket(buffer, frame)) { // On successful decoding execute the given command - TbotsProto_PowerPulseControl control = frame.power_msg.power_control; - executor->execute(control); + if (frame.which_power_msg == TbotsProto_PowerFrame_power_control_tag) + { + executor->execute(frame.power_msg.power_control); + } + else if (frame.which_power_msg == TbotsProto_PowerFrame_dribbler_control_tag) + { + dribbler->dribble(frame.power_msg.dribble_control.dribble_speed); + } } buffer.clear(); From 013933878975f4dd74ff52d8a6f1751de3934b0a Mon Sep 17 00:00:00 2001 From: Grayson Date: Mon, 25 May 2026 16:41:55 -0700 Subject: [PATCH 20/34] route dribbler commands from motorservice --- .../message_translation/power_frame_msg.hpp | 6 +++ src/proto/power_frame_msg.proto | 2 +- src/software/embedded/BUILD | 1 + .../stspin_motor_controller.cpp | 6 ++- .../stspin_motor_controller.h | 5 +- src/software/embedded/services/BUILD | 1 - src/software/embedded/services/motor.cpp | 17 ++----- src/software/embedded/services/motor.h | 3 +- src/software/embedded/services/power.cpp | 51 +++++++++++++++++++ src/software/embedded/services/power.h | 24 +++++++++ src/software/embedded/thunderloop.cpp | 20 ++++++-- 11 files changed, 112 insertions(+), 24 deletions(-) diff --git a/src/proto/message_translation/power_frame_msg.hpp b/src/proto/message_translation/power_frame_msg.hpp index 6caa0e3de9..2161617e82 100644 --- a/src/proto/message_translation/power_frame_msg.hpp +++ b/src/proto/message_translation/power_frame_msg.hpp @@ -316,4 +316,10 @@ std::unique_ptr inline createTbotsPowerStatus( proto_status->ParseFromString(std::string(buffer.begin(), buffer.end())); return proto_status; } + +TbotsProto_DribblerControl inline createNanoPbDribblerControl(int rpm) { + TbotsProto_DribblerControl control = TbotsProto_DribblerControl_init_default; + control.dribbler_speed = rpm; + return control; +} #endif diff --git a/src/proto/power_frame_msg.proto b/src/proto/power_frame_msg.proto index 2ef85cf2d7..047bc74c92 100644 --- a/src/proto/power_frame_msg.proto +++ b/src/proto/power_frame_msg.proto @@ -34,7 +34,7 @@ message PowerPulseControl message DribblerControl { - uint8 dribbler_speed = 1; + uint32 dribbler_speed = 1; // Changed from uint8 to have better rpm control } message PowerFrame diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 5ee03cb54c..f4d8486575 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -58,6 +58,7 @@ cc_library( deps = [ ":primitive_executor", "//proto:tbots_cc_proto", + "//software/embedded/motor_controller:motor_board", "//software/embedded/services:motor", "//software/embedded/services:power", "//software/embedded/services/network", diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 5a34acdbaf..d773103a61 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -22,8 +22,9 @@ using Crc8Autosar = crc_utils::crc; StSpinMotorController::StSpinMotorController( - const robot_constants::RobotConstants& robot_constants) + const robot_constants::RobotConstants& robot_constants, PowerServiceWithDribble& power_service) : robot_constants_(robot_constants), + power_service_(power_service), reset_gpio_(std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, GpioState::HIGH)) { @@ -177,7 +178,8 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, { if (motor == MotorIndex::DRIBBLER) { - return 0; + // Here target_velocity is treated as angular velocity with unit RPM + power_service_.dribble(target_velocity); } const auto outgoing_frame = SetTargetSpeedFrame{ diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index f3cb911c64..6967f44c40 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -5,6 +5,7 @@ #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" +#include "software/embedded/services/power.h" /** * Motor controller for controlling our 6th generation STSPIN motor drivers. @@ -17,7 +18,7 @@ class StSpinMotorController : public MotorController { public: explicit StSpinMotorController( - const robot_constants::RobotConstants& robot_constants); + const robot_constants::RobotConstants& robot_constants, PowerServiceWithDribble& power_service); void setup() override; @@ -89,6 +90,8 @@ class StSpinMotorController : public MotorController std::unordered_map motor_status_; + PowerServiceWithDribble& power_service_; + /** * Opens a SPI file descriptor for the given motor. * diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index 528b1eedc9..f0fcd0077d 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -14,7 +14,6 @@ cc_library( "//shared:robot_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", diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 6c838b0c5a..847c2d9d3c 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -1,14 +1,13 @@ #include "software/embedded/services/motor.h" #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" -MotorService::MotorService(const robot_constants::RobotConstants& robot_constants) +MotorService::MotorService(const robot_constants::RobotConstants& robot_constants, std::unique_ptr motor_controller) : robot_constants_(robot_constants), - motor_controller_(setupMotorController()), + motor_controller_(motor_controller), euclidean_to_four_wheel_(robot_constants), dribbler_target_rpm_(0), // placeholder, move to power_service drive_motor_mps_per_rpm_(2 * M_PI * robot_constants.wheel_radius_meters / 60), @@ -29,17 +28,7 @@ void MotorService::reset() motor_controller_->reset(); } -std::unique_ptr MotorService::setupMotorController() -{ - if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) - { - return std::make_unique(); - } - else - { - return std::make_unique(robot_constants_); - } -} + TbotsProto::MotorStatus MotorService::createMotorStatus( const WheelSpace_t& current_wheel_velocities, const double dribbler_rpm) const diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index 445fb03ff5..a56d106c97 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -24,8 +24,9 @@ class MotorService * Service that interacts with the motors. * * @param robot_constants The robot constants + * @param motor_controller The motor controller */ - explicit MotorService(const robot_constants::RobotConstants& robot_constants); + explicit MotorService(const robot_constants::RobotConstants& robot_constants, std::unique_ptr motor_controller); /** * Polls the motor service to execute the given motor control command and diff --git a/src/software/embedded/services/power.cpp b/src/software/embedded/services/power.cpp index c9c98f1ff8..f9d60bcde5 100644 --- a/src/software/embedded/services/power.cpp +++ b/src/software/embedded/services/power.cpp @@ -83,3 +83,54 @@ TbotsProto::PowerStatus PowerService::poll(const TbotsProto::PowerControl& comma createNanoPbPowerPulseControl(command, kick_coeff, kick_constant, chip_constant); return *createTbotsPowerStatus(status); } + +void PowerServiceWithDribble::dribble(int rpm) { + dribble_command = createNanoPbDribblerControl(rpm); + _new_dribble_command = true; +} + +void PowerServiceWithDribble::tick() +{ + std::vector power_status; + try + { + uart->flushSerialPort(uart->flush_receive); + power_status = uart->serialRead(READ_BUFFER_SIZE); + } + catch (std::exception& e) + { + LOG(FATAL) << "Read thread has crashed" << e.what(); + } + + TbotsProto_PowerFrame status_frame = TbotsProto_PowerFrame_init_default; + if (!unmarshalUartPacket(power_status, status_frame)) + { + LOG(WARNING) << "Unmarshal failed"; + } + else + { + status = status_frame.power_msg.power_status; + } + if (_new_dribble_command) { + auto command = dribble_command.load(std::memory_order_relaxed); // get value atomically + } else { + auto command = + nanopb_command.load(std::memory_order_relaxed); // get value atomically + } + auto frame = createUartFrame(command); + auto power_command_buffer = marshallUartPacket(frame); + + try + { + // Write power command + uart->flushSerialPort(uart->flush_send); + if (!uart->serialWrite(power_command_buffer)) + { + LOG(WARNING) << "Writing power command failed."; + } + } + catch (std::exception& e) + { + LOG(FATAL) << "ESP32 has disconnected. Power service has crashed" << e.what(); + } +} \ No newline at end of file diff --git a/src/software/embedded/services/power.h b/src/software/embedded/services/power.h index b479e5eec2..98b2e95265 100644 --- a/src/software/embedded/services/power.h +++ b/src/software/embedded/services/power.h @@ -38,6 +38,11 @@ class PowerService */ void tick(); + /** + * Set dribbler RPM + */ + void dribble(int rpm); + private: /** * Initiates timer for serial reading @@ -58,3 +63,22 @@ class PowerService // Required flag to exit power service cleanly bool is_running = true; }; + +/** + * A separate power service that also controls the dribbler. + */ +class PowerServiceWithDribble : public PowerService { + public: + /** + * Handler method called every time the timer expires a new read is requested + */ + override void tick(); + + /** + * Set dribbler RPM + */ + void dribble(int rpm); + private: + bool _new_dribble_command = false; + std::atomic dribble_command; +} \ No newline at end of file diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 7086c57882..5409e8049d 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -15,6 +15,7 @@ #include "software/networking/tbots_network_exception.h" #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" +#include "software/embedded/motor_controller/motor_board.h" /** * https://web.archive.org/web/20210308013218/https://rt.wiki.kernel.org/index.php/Squarewave-example @@ -114,11 +115,22 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, LOG(INFO) << "THUNDERLOOP: Network Service initialized! Next initializing Power Service"; - power_service_ = std::make_unique(); - LOG(INFO) - << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; + - motor_service_ = std::make_unique(robot_constants); + if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) + { + power_service_ = std::make_unique(); + LOG(INFO) + << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; + motor_service_ = std::make_unique(robot_constants,std::make_unique()); + } + else + { + power_service_ = std::make_unique(); + LOG(INFO) + << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; + motor_service_ = std::make_unique(robot_constants, std::make_unique(robot_constants, power_service_)); + } g_motor_service = motor_service_.get(); motor_service_->setup(); LOG(INFO) << "THUNDERLOOP: Motor Service initialized!"; From 32c531878fa5a75ab6138af43ee5115ad2705eea Mon Sep 17 00:00:00 2001 From: Grayson Date: Wed, 27 May 2026 17:38:05 -0700 Subject: [PATCH 21/34] dribble ramping --- src/software/embedded/services/power.cpp | 1 + src/software/power/powerloop_main.cc | 17 ++++++++++++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/software/embedded/services/power.cpp b/src/software/embedded/services/power.cpp index f9d60bcde5..19c2c1ccd1 100644 --- a/src/software/embedded/services/power.cpp +++ b/src/software/embedded/services/power.cpp @@ -113,6 +113,7 @@ void PowerServiceWithDribble::tick() } if (_new_dribble_command) { auto command = dribble_command.load(std::memory_order_relaxed); // get value atomically + _new_dribble_command = false; } else { auto command = nanopb_command.load(std::memory_order_relaxed); // get value atomically diff --git a/src/software/power/powerloop_main.cc b/src/software/power/powerloop_main.cc index a32505d7bb..9d5721aef5 100644 --- a/src/software/power/powerloop_main.cc +++ b/src/software/power/powerloop_main.cc @@ -37,6 +37,11 @@ std::shared_ptr geneva; std::shared_ptr executor; std::shared_ptr dribbler; +#define RAMP_FACTOR 4 +#define DRIBBLER_MAX_SPEED 11040 // Max RPM from spec +int dribble_target; +int dribble_speed; + void setup() { Serial.begin(460800, SERIAL_8N1); @@ -49,6 +54,8 @@ void setup() executor = std::make_shared(charger, chicker, geneva); dribbler = std::make_shared(); charger->chargeCapacitors(); + dribble_speed = 0; + dribble_target = 0; } void loop() @@ -72,7 +79,7 @@ void loop() } else if (frame.which_power_msg == TbotsProto_PowerFrame_dribbler_control_tag) { - dribbler->dribble(frame.power_msg.dribble_control.dribble_speed); + dribble_target = frame.power_msg.dribbler_control.dribble_speed; } } @@ -96,6 +103,14 @@ void loop() monitor->getCurrentDrawAmp(), geneva->getCurrentSlot(), sequence_num++, chicker->getBreakBeamTripped()); + if (dribble_target < dribble_speed) { + dribble_speed = dribble_target; + } else { + // Ramp to speed + dribble_speed = dribble_speed + (dribble_target-dribble_speed)/RAMP_FACTOR; + } + dribbler->dribble(dribble_speed/DRIBBLER_MAX_SPEED*255); + // Write sensor values out to Serial TbotsProto_PowerFrame status_frame = createUartFrame(status); std::vector packet = marshallUartPacket(status_frame); From 674d6de8f9e665c1893f7abc0313ba396dad53d1 Mon Sep 17 00:00:00 2001 From: Grayson Date: Wed, 27 May 2026 20:21:29 -0700 Subject: [PATCH 22/34] fix compilation --- src/proto/message_translation/power_frame_msg.hpp | 7 +++++++ src/software/embedded/motor_controller/BUILD | 1 + .../embedded/motor_controller/stspin_motor_controller.cpp | 4 ++-- src/software/embedded/services/motor.cpp | 2 +- src/software/embedded/services/power.cpp | 7 ++++--- src/software/embedded/services/power.h | 6 +++--- src/software/embedded/thunderloop.cpp | 2 ++ src/software/power/dribbler.cpp | 2 +- 8 files changed, 21 insertions(+), 10 deletions(-) diff --git a/src/proto/message_translation/power_frame_msg.hpp b/src/proto/message_translation/power_frame_msg.hpp index 2161617e82..d50d2d4132 100644 --- a/src/proto/message_translation/power_frame_msg.hpp +++ b/src/proto/message_translation/power_frame_msg.hpp @@ -85,6 +85,13 @@ void inline setPowerMsg(TbotsProto_PowerFrame& frame, frame.power_msg.power_status = status; } +void inline setPowerMsg(TbotsProto_PowerFrame& frame, + const TbotsProto_DribblerControl& dribble_control) +{ + frame.which_power_msg = TbotsProto_PowerFrame_dribbler_control_tag; + frame.power_msg.dribbler_control = dribble_control; +} + /** * Creates a nanopb power status msg with provided fields * diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index 26518edbfd..e5d5661a7d 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -32,6 +32,7 @@ cc_library( ":motor_fault_indicator", ":motor_index", ":stspin_types", + "//software/embedded/services:power", "//proto/message_translation:tbots_protobuf", "//software/embedded:spi_utils", "//software/embedded/gpio", diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index d773103a61..4b2e76fafc 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -24,9 +24,9 @@ using Crc8Autosar = crc_utils::crc; StSpinMotorController::StSpinMotorController( const robot_constants::RobotConstants& robot_constants, PowerServiceWithDribble& power_service) : robot_constants_(robot_constants), - power_service_(power_service), reset_gpio_(std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, - GpioState::HIGH)) + GpioState::HIGH)), + power_service_(power_service) { for (const MotorIndex motor : driveMotors()) { diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 847c2d9d3c..81dd3a4473 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -7,7 +7,7 @@ MotorService::MotorService(const robot_constants::RobotConstants& robot_constants, std::unique_ptr motor_controller) : robot_constants_(robot_constants), - motor_controller_(motor_controller), + motor_controller_(std::move(motor_controller)), euclidean_to_four_wheel_(robot_constants), dribbler_target_rpm_(0), // placeholder, move to power_service drive_motor_mps_per_rpm_(2 * M_PI * robot_constants.wheel_radius_meters / 60), diff --git a/src/software/embedded/services/power.cpp b/src/software/embedded/services/power.cpp index 19c2c1ccd1..89401e7d6c 100644 --- a/src/software/embedded/services/power.cpp +++ b/src/software/embedded/services/power.cpp @@ -10,7 +10,7 @@ PowerService::PowerService() nanopb_command = createNanoPbPowerPulseControl(TbotsProto::PowerControl(), 0.0, 0, 0); if (!boost::filesystem::exists(DEVICE_SERIAL_PORT)) - { + { throw std::runtime_error("USB not plugged into the Raspberry Pi"); } this->uart = std::make_unique(BAUD_RATE, DEVICE_SERIAL_PORT); @@ -111,15 +111,16 @@ void PowerServiceWithDribble::tick() { status = status_frame.power_msg.power_status; } + std::vector power_command_buffer; if (_new_dribble_command) { auto command = dribble_command.load(std::memory_order_relaxed); // get value atomically _new_dribble_command = false; + power_command_buffer = marshallUartPacket(createUartFrame(command)); } else { auto command = nanopb_command.load(std::memory_order_relaxed); // get value atomically + power_command_buffer = marshallUartPacket(createUartFrame(command)); } - auto frame = createUartFrame(command); - auto power_command_buffer = marshallUartPacket(frame); try { diff --git a/src/software/embedded/services/power.h b/src/software/embedded/services/power.h index 98b2e95265..ba30190cba 100644 --- a/src/software/embedded/services/power.h +++ b/src/software/embedded/services/power.h @@ -36,7 +36,7 @@ class PowerService /** * Handler method called every time the timer expires a new read is requested */ - void tick(); + virtual void tick(); /** * Set dribbler RPM @@ -72,7 +72,7 @@ class PowerServiceWithDribble : public PowerService { /** * Handler method called every time the timer expires a new read is requested */ - override void tick(); + void tick() override; /** * Set dribbler RPM @@ -81,4 +81,4 @@ class PowerServiceWithDribble : public PowerService { private: bool _new_dribble_command = false; std::atomic dribble_command; -} \ No newline at end of file +}; \ No newline at end of file diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 4512617ea7..be49c6e693 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -18,6 +18,8 @@ #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.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" /** * https://web.archive.org/web/20210308013218/https://rt.wiki.kernel.org/index.php/Squarewave-example diff --git a/src/software/power/dribbler.cpp b/src/software/power/dribbler.cpp index d82d37e1a1..f8268d5721 100644 --- a/src/software/power/dribbler.cpp +++ b/src/software/power/dribbler.cpp @@ -1,4 +1,4 @@ -#include "dribbler.h" +#include "software/power/dribbler.h" Dribbler::Dribbler() { From 12a8d46ffd268fad1ff3c7e0dc93e5174f51538f Mon Sep 17 00:00:00 2001 From: Grayson Date: Wed, 27 May 2026 23:05:03 -0700 Subject: [PATCH 23/34] 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 24/34] 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 25/34] 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 9e243ddcd245cb01f157e0d09412f75a4ef66b78 Mon Sep 17 00:00:00 2001 From: Grayson Date: Wed, 27 May 2026 23:23:00 -0700 Subject: [PATCH 26/34] ramping adjustments --- src/software/power/powerloop_main.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/power/powerloop_main.cc b/src/software/power/powerloop_main.cc index 9d5721aef5..ea23ab378c 100644 --- a/src/software/power/powerloop_main.cc +++ b/src/software/power/powerloop_main.cc @@ -103,11 +103,11 @@ void loop() monitor->getCurrentDrawAmp(), geneva->getCurrentSlot(), sequence_num++, chicker->getBreakBeamTripped()); - if (dribble_target < dribble_speed) { + if (dribble_target <= dribble_speed) { dribble_speed = dribble_target; } else { // Ramp to speed - dribble_speed = dribble_speed + (dribble_target-dribble_speed)/RAMP_FACTOR; + dribble_speed = dribble_speed + (dribble_target-dribble_speed)/RAMP_FACTOR + 1; } dribbler->dribble(dribble_speed/DRIBBLER_MAX_SPEED*255); From 19b95d15ebba6e3e9f918e66150092b40b07687b Mon Sep 17 00:00:00 2001 From: Grayson Date: Thu, 28 May 2026 16:39:01 -0700 Subject: [PATCH 27/34] 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 4b2e76fafc..13a47e41ad 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -85,7 +85,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"; @@ -198,7 +199,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 6967f44c40..4e7474bf69 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -33,6 +33,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 0d732beec0d931015cfe91d8a4d0da80fefc8c58 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Fri, 29 May 2026 05:19:22 +0000 Subject: [PATCH 28/34] [pre-commit.ci lite] apply automatic fixes --- .../message_translation/power_frame_msg.hpp | 7 +++--- src/proto/power_frame_msg.proto | 2 +- src/software/embedded/BUILD | 2 +- src/software/embedded/motor_controller/BUILD | 2 +- .../stspin_motor_controller.cpp | 3 ++- .../stspin_motor_controller.h | 4 ++-- src/software/embedded/services/motor.cpp | 3 ++- src/software/embedded/services/motor.h | 3 ++- src/software/embedded/services/power.cpp | 19 +++++++++------ src/software/embedded/services/power.h | 6 +++-- src/software/embedded/thunderloop.cpp | 15 +++++++----- src/software/embedded/thunderloop.h | 2 +- src/software/power/dribbler.cpp | 5 ++-- src/software/power/dribbler.h | 6 ++--- src/software/power/pins.h | 2 +- src/software/power/powerloop_main.cc | 23 +++++++++++-------- 16 files changed, 62 insertions(+), 42 deletions(-) diff --git a/src/proto/message_translation/power_frame_msg.hpp b/src/proto/message_translation/power_frame_msg.hpp index d50d2d4132..c180d7e4af 100644 --- a/src/proto/message_translation/power_frame_msg.hpp +++ b/src/proto/message_translation/power_frame_msg.hpp @@ -88,7 +88,7 @@ void inline setPowerMsg(TbotsProto_PowerFrame& frame, void inline setPowerMsg(TbotsProto_PowerFrame& frame, const TbotsProto_DribblerControl& dribble_control) { - frame.which_power_msg = TbotsProto_PowerFrame_dribbler_control_tag; + frame.which_power_msg = TbotsProto_PowerFrame_dribbler_control_tag; frame.power_msg.dribbler_control = dribble_control; } @@ -324,9 +324,10 @@ std::unique_ptr inline createTbotsPowerStatus( return proto_status; } -TbotsProto_DribblerControl inline createNanoPbDribblerControl(int rpm) { +TbotsProto_DribblerControl inline createNanoPbDribblerControl(int rpm) +{ TbotsProto_DribblerControl control = TbotsProto_DribblerControl_init_default; - control.dribbler_speed = rpm; + control.dribbler_speed = rpm; return control; } #endif diff --git a/src/proto/power_frame_msg.proto b/src/proto/power_frame_msg.proto index 047bc74c92..3a2b91d2c4 100644 --- a/src/proto/power_frame_msg.proto +++ b/src/proto/power_frame_msg.proto @@ -34,7 +34,7 @@ message PowerPulseControl message DribblerControl { - uint32 dribbler_speed = 1; // Changed from uint8 to have better rpm control + uint32 dribbler_speed = 1; // Changed from uint8 to have better rpm control } message PowerFrame diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 61a32b451e..0651ff70ee 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -58,6 +58,7 @@ cc_library( deps = [ ":primitive_executor", "//proto:tbots_cc_proto", + "//software/embedded/motor_controller", "//software/embedded/motor_controller:motor_board", "//software/embedded/services:imu", "//software/embedded/services:motor", @@ -67,7 +68,6 @@ cc_library( "//software/logger:network_logger", "//software/tracy:tracy_constants", "//software/util/scoped_timespec_timer", - "//software/embedded/motor_controller:motor_controller", "@tracy", ], ) diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index e5d5661a7d..588398fb60 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -32,10 +32,10 @@ cc_library( ":motor_fault_indicator", ":motor_index", ":stspin_types", - "//software/embedded/services:power", "//proto/message_translation:tbots_protobuf", "//software/embedded:spi_utils", "//software/embedded/gpio", + "//software/embedded/services:power", "//software/logger", "//software/physics:euclidean_to_wheel", "@cppcrc", diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 96f80b966d..7e92d86fbc 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -22,7 +22,8 @@ using Crc8Autosar = crc_utils::crc; StSpinMotorController::StSpinMotorController( - const robot_constants::RobotConstants& robot_constants, PowerServiceWithDribble& power_service) + const robot_constants::RobotConstants& robot_constants, + PowerServiceWithDribble& power_service) : robot_constants_(robot_constants), reset_gpio_(std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, GpioState::HIGH)), diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 5540ede0f1..270af5983f 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -17,8 +17,8 @@ class StSpinMotorController : public MotorController { public: - explicit StSpinMotorController( - const robot_constants::RobotConstants& robot_constants, PowerServiceWithDribble& power_service); + explicit StSpinMotorController(const robot_constants::RobotConstants& robot_constants, + PowerServiceWithDribble& power_service); void setup() override; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 38c2014b01..f87237e77e 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -6,7 +6,8 @@ #include "software/embedded/motor_controller/tmc_motor_controller.h" #include "software/logger/logger.h" -MotorService::MotorService(const robot_constants::RobotConstants& robot_constants, std::unique_ptr motor_controller) +MotorService::MotorService(const robot_constants::RobotConstants& robot_constants, + std::unique_ptr motor_controller) : robot_constants_(robot_constants), motor_controller_(std::move(motor_controller)), euclidean_to_four_wheel_(robot_constants), diff --git a/src/software/embedded/services/motor.h b/src/software/embedded/services/motor.h index a56d106c97..19e7f81570 100644 --- a/src/software/embedded/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -26,7 +26,8 @@ class MotorService * @param robot_constants The robot constants * @param motor_controller The motor controller */ - explicit MotorService(const robot_constants::RobotConstants& robot_constants, std::unique_ptr motor_controller); + explicit MotorService(const robot_constants::RobotConstants& robot_constants, + std::unique_ptr motor_controller); /** * Polls the motor service to execute the given motor control command and diff --git a/src/software/embedded/services/power.cpp b/src/software/embedded/services/power.cpp index 89401e7d6c..19a7749311 100644 --- a/src/software/embedded/services/power.cpp +++ b/src/software/embedded/services/power.cpp @@ -10,7 +10,7 @@ PowerService::PowerService() nanopb_command = createNanoPbPowerPulseControl(TbotsProto::PowerControl(), 0.0, 0, 0); if (!boost::filesystem::exists(DEVICE_SERIAL_PORT)) - { + { throw std::runtime_error("USB not plugged into the Raspberry Pi"); } this->uart = std::make_unique(BAUD_RATE, DEVICE_SERIAL_PORT); @@ -84,8 +84,9 @@ TbotsProto::PowerStatus PowerService::poll(const TbotsProto::PowerControl& comma return *createTbotsPowerStatus(status); } -void PowerServiceWithDribble::dribble(int rpm) { - dribble_command = createNanoPbDribblerControl(rpm); +void PowerServiceWithDribble::dribble(int rpm) +{ + dribble_command = createNanoPbDribblerControl(rpm); _new_dribble_command = true; } @@ -112,11 +113,15 @@ void PowerServiceWithDribble::tick() status = status_frame.power_msg.power_status; } std::vector power_command_buffer; - if (_new_dribble_command) { - auto command = dribble_command.load(std::memory_order_relaxed); // get value atomically + if (_new_dribble_command) + { + auto command = + dribble_command.load(std::memory_order_relaxed); // get value atomically _new_dribble_command = false; power_command_buffer = marshallUartPacket(createUartFrame(command)); - } else { + } + else + { auto command = nanopb_command.load(std::memory_order_relaxed); // get value atomically power_command_buffer = marshallUartPacket(createUartFrame(command)); @@ -135,4 +140,4 @@ void PowerServiceWithDribble::tick() { LOG(FATAL) << "ESP32 has disconnected. Power service has crashed" << e.what(); } -} \ No newline at end of file +} diff --git a/src/software/embedded/services/power.h b/src/software/embedded/services/power.h index ba30190cba..b3cb5f730d 100644 --- a/src/software/embedded/services/power.h +++ b/src/software/embedded/services/power.h @@ -67,7 +67,8 @@ class PowerService /** * A separate power service that also controls the dribbler. */ -class PowerServiceWithDribble : public PowerService { +class PowerServiceWithDribble : public PowerService +{ public: /** * Handler method called every time the timer expires a new read is requested @@ -78,7 +79,8 @@ class PowerServiceWithDribble : public PowerService { * Set dribbler RPM */ void dribble(int rpm); + private: bool _new_dribble_command = false; std::atomic dribble_command; -}; \ No newline at end of file +}; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index be49c6e693..331107c0ec 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -9,6 +9,9 @@ #include "proto/tbots_software_msgs.pb.h" #include "shared/constants.h" #include "software/constants.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/embedded/primitive_executor.h" #include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" @@ -17,9 +20,6 @@ #include "software/networking/tbots_network_exception.h" #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.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" /** * https://web.archive.org/web/20210308013218/https://rt.wiki.kernel.org/index.php/Squarewave-example @@ -120,21 +120,24 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, LOG(INFO) << "THUNDERLOOP: Network Service initialized! Next initializing Power Service"; - + if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) { power_service_ = std::make_unique(); LOG(INFO) << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; - motor_service_ = std::make_unique(robot_constants,std::make_unique()); + motor_service_ = std::make_unique( + robot_constants, std::make_unique()); } else { power_service_ = std::make_unique(); LOG(INFO) << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; - motor_service_ = std::make_unique(robot_constants, std::make_unique(robot_constants, power_service_)); + motor_service_ = std::make_unique( + robot_constants, + std::make_unique(robot_constants, power_service_)); } g_motor_service = motor_service_.get(); motor_service_->setup(); 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"; }; diff --git a/src/software/power/dribbler.cpp b/src/software/power/dribbler.cpp index f8268d5721..dd5716884c 100644 --- a/src/software/power/dribbler.cpp +++ b/src/software/power/dribbler.cpp @@ -5,6 +5,7 @@ Dribbler::Dribbler() pinMode(DRIBBLER_PIN, OUTPUT); } -void Dribbler::dribble(uint32_t speed) { +void Dribbler::dribble(uint32_t speed) +{ analogWrite(DRIBBLER_PIN, speed); -} \ No newline at end of file +} diff --git a/src/software/power/dribbler.h b/src/software/power/dribbler.h index 355e902104..150301d58f 100644 --- a/src/software/power/dribbler.h +++ b/src/software/power/dribbler.h @@ -5,9 +5,9 @@ /** * Represents the dribbler on the power board */ -class Dribbler +class Dribbler { - public: + public: /** * Creates a dribbler, setting up pins. */ @@ -17,4 +17,4 @@ class Dribbler * Sets the dribbler to the given duty cycle from 0 (always off) to 255 (always on) */ static void dribble(uint32_t speed); -} \ No newline at end of file +} diff --git a/src/software/power/pins.h b/src/software/power/pins.h index 2d18caf001..bc958d251c 100644 --- a/src/software/power/pins.h +++ b/src/software/power/pins.h @@ -32,7 +32,7 @@ const uint8_t PM_SDA = 13; const uint8_t PM_SCL = 14; // Dribbler -const uint8_t DRIBBLER_PIN = 0; // When elec has a pin for this +const uint8_t DRIBBLER_PIN = 0; // When elec has a pin for this // Timers const uint8_t CHICKER_PULSE_TIMER = 0; diff --git a/src/software/power/powerloop_main.cc b/src/software/power/powerloop_main.cc index ea23ab378c..58e5650431 100644 --- a/src/software/power/powerloop_main.cc +++ b/src/software/power/powerloop_main.cc @@ -38,7 +38,7 @@ std::shared_ptr executor; std::shared_ptr dribbler; #define RAMP_FACTOR 4 -#define DRIBBLER_MAX_SPEED 11040 // Max RPM from spec +#define DRIBBLER_MAX_SPEED 11040 // Max RPM from spec int dribble_target; int dribble_speed; @@ -54,7 +54,7 @@ void setup() executor = std::make_shared(charger, chicker, geneva); dribbler = std::make_shared(); charger->chargeCapacitors(); - dribble_speed = 0; + dribble_speed = 0; dribble_target = 0; } @@ -73,11 +73,12 @@ void loop() if (unmarshalUartPacket(buffer, frame)) { // On successful decoding execute the given command - if (frame.which_power_msg == TbotsProto_PowerFrame_power_control_tag) + if (frame.which_power_msg == TbotsProto_PowerFrame_power_control_tag) { executor->execute(frame.power_msg.power_control); - } - else if (frame.which_power_msg == TbotsProto_PowerFrame_dribbler_control_tag) + } + else if (frame.which_power_msg == + TbotsProto_PowerFrame_dribbler_control_tag) { dribble_target = frame.power_msg.dribbler_control.dribble_speed; } @@ -103,13 +104,17 @@ void loop() monitor->getCurrentDrawAmp(), geneva->getCurrentSlot(), sequence_num++, chicker->getBreakBeamTripped()); - if (dribble_target <= dribble_speed) { + if (dribble_target <= dribble_speed) + { dribble_speed = dribble_target; - } else { + } + else + { // Ramp to speed - dribble_speed = dribble_speed + (dribble_target-dribble_speed)/RAMP_FACTOR + 1; + dribble_speed = + dribble_speed + (dribble_target - dribble_speed) / RAMP_FACTOR + 1; } - dribbler->dribble(dribble_speed/DRIBBLER_MAX_SPEED*255); + dribbler->dribble(dribble_speed / DRIBBLER_MAX_SPEED * 255); // Write sensor values out to Serial TbotsProto_PowerFrame status_frame = createUartFrame(status); From 102e115b3f1400476496949dfb4d1b65583436c8 Mon Sep 17 00:00:00 2001 From: Grayson Date: Fri, 29 May 2026 00:06:25 -0700 Subject: [PATCH 29/34] build fixes --- src/software/power/BUILD | 8 ++++++++ src/software/power/dribbler.cpp | 2 +- src/software/power/dribbler.h | 2 +- src/software/power/powerloop_main.cc | 14 +++++++------- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/software/power/BUILD b/src/software/power/BUILD index f39689a58d..37b367c8e9 100644 --- a/src/software/power/BUILD +++ b/src/software/power/BUILD @@ -16,6 +16,13 @@ platformio_library( deps = [":pins"], ) +platformio_library( + name = "dribbler", + src = "dribbler.cpp", + hdr = "dribbler.h", + deps = [":pins"], +) + platformio_library( name = "geneva", src = "geneva.cpp", @@ -73,6 +80,7 @@ platformio_project( ":charger", ":chicker", ":control_executor", + ":dribbler", ":geneva", ":pins", ":power_monitor", diff --git a/src/software/power/dribbler.cpp b/src/software/power/dribbler.cpp index f8268d5721..d82d37e1a1 100644 --- a/src/software/power/dribbler.cpp +++ b/src/software/power/dribbler.cpp @@ -1,4 +1,4 @@ -#include "software/power/dribbler.h" +#include "dribbler.h" Dribbler::Dribbler() { diff --git a/src/software/power/dribbler.h b/src/software/power/dribbler.h index 355e902104..08e9e75903 100644 --- a/src/software/power/dribbler.h +++ b/src/software/power/dribbler.h @@ -17,4 +17,4 @@ class Dribbler * Sets the dribbler to the given duty cycle from 0 (always off) to 255 (always on) */ static void dribble(uint32_t speed); -} \ No newline at end of file +}; \ No newline at end of file diff --git a/src/software/power/powerloop_main.cc b/src/software/power/powerloop_main.cc index ea23ab378c..6e5414e938 100644 --- a/src/software/power/powerloop_main.cc +++ b/src/software/power/powerloop_main.cc @@ -40,7 +40,7 @@ std::shared_ptr dribbler; #define RAMP_FACTOR 4 #define DRIBBLER_MAX_SPEED 11040 // Max RPM from spec int dribble_target; -int dribble_speed; +int dribbler_speed; void setup() { @@ -54,7 +54,7 @@ void setup() executor = std::make_shared(charger, chicker, geneva); dribbler = std::make_shared(); charger->chargeCapacitors(); - dribble_speed = 0; + dribbler_speed = 0; dribble_target = 0; } @@ -79,7 +79,7 @@ void loop() } else if (frame.which_power_msg == TbotsProto_PowerFrame_dribbler_control_tag) { - dribble_target = frame.power_msg.dribbler_control.dribble_speed; + dribble_target = frame.power_msg.dribbler_control.dribbler_speed; } } @@ -103,13 +103,13 @@ void loop() monitor->getCurrentDrawAmp(), geneva->getCurrentSlot(), sequence_num++, chicker->getBreakBeamTripped()); - if (dribble_target <= dribble_speed) { - dribble_speed = dribble_target; + if (dribble_target <= dribbler_speed) { + dribbler_speed = dribble_target; } else { // Ramp to speed - dribble_speed = dribble_speed + (dribble_target-dribble_speed)/RAMP_FACTOR + 1; + dribbler_speed = dribbler_speed + (dribble_target-dribbler_speed)/RAMP_FACTOR + 1; } - dribbler->dribble(dribble_speed/DRIBBLER_MAX_SPEED*255); + dribbler->dribble(dribbler_speed/DRIBBLER_MAX_SPEED*255); // Write sensor values out to Serial TbotsProto_PowerFrame status_frame = createUartFrame(status); From 2a721ee8bcf9578dfcdb1ade72adde72960034ad Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Fri, 29 May 2026 07:13:24 +0000 Subject: [PATCH 30/34] [pre-commit.ci lite] apply automatic fixes --- .../embedded/ansible/requirements_lock.txt | 331 ------------------ src/software/power/powerloop_main.cc | 12 +- 2 files changed, 8 insertions(+), 335 deletions(-) diff --git a/src/software/embedded/ansible/requirements_lock.txt b/src/software/embedded/ansible/requirements_lock.txt index 77dfeb01c6..e69de29bb2 100644 --- a/src/software/embedded/ansible/requirements_lock.txt +++ b/src/software/embedded/ansible/requirements_lock.txt @@ -1,331 +0,0 @@ -# -# This file is autogenerated by pip-compile with Python 3.12 -# by the following command: -# -# bazel run //software/embedded/ansible:requirements.update -# -ansible==11.9.0 \ - --hash=sha256:528ca5a408f11cf1fea00daea7570e68d40e167be38b90c119a7cb45729e4921 \ - --hash=sha256:79b087ef38105b93e0e092e7013a0f840e154a6a8ce9b5fddd1b47593adc542a - # via -r software/embedded/ansible/requirements.in -ansible-core==2.18.9 \ - --hash=sha256:25206e1aac3bd30d95649a5ccf0d3646461d02b4dc265b5959e33b7ccd6f23f8 \ - --hash=sha256:a5f4a02aad5843e990ff7be1b92dd658a8b230de713ea643920e683ebf980da1 - # via ansible -cffi==2.0.0 \ - --hash=sha256:00bdf7acc5f795150faa6957054fbbca2439db2f775ce831222b66f192f03beb \ - --hash=sha256:07b271772c100085dd28b74fa0cd81c8fb1a3ba18b21e03d7c27f3436a10606b \ - --hash=sha256:087067fa8953339c723661eda6b54bc98c5625757ea62e95eb4898ad5e776e9f \ - --hash=sha256:0a1527a803f0a659de1af2e1fd700213caba79377e27e4693648c2923da066f9 \ - --hash=sha256:0cf2d91ecc3fcc0625c2c530fe004f82c110405f101548512cce44322fa8ac44 \ - --hash=sha256:0f6084a0ea23d05d20c3edcda20c3d006f9b6f3fefeac38f59262e10cef47ee2 \ - --hash=sha256:12873ca6cb9b0f0d3a0da705d6086fe911591737a59f28b7936bdfed27c0d47c \ - --hash=sha256:19f705ada2530c1167abacb171925dd886168931e0a7b78f5bffcae5c6b5be75 \ - --hash=sha256:1cd13c99ce269b3ed80b417dcd591415d3372bcac067009b6e0f59c7d4015e65 \ - --hash=sha256:1e3a615586f05fc4065a8b22b8152f0c1b00cdbc60596d187c2a74f9e3036e4e \ - --hash=sha256:1f72fb8906754ac8a2cc3f9f5aaa298070652a0ffae577e0ea9bd480dc3c931a \ - --hash=sha256:1fc9ea04857caf665289b7a75923f2c6ed559b8298a1b8c49e59f7dd95c8481e \ - --hash=sha256:203a48d1fb583fc7d78a4c6655692963b860a417c0528492a6bc21f1aaefab25 \ - --hash=sha256:2081580ebb843f759b9f617314a24ed5738c51d2aee65d31e02f6f7a2b97707a \ - --hash=sha256:21d1152871b019407d8ac3985f6775c079416c282e431a4da6afe7aefd2bccbe \ - --hash=sha256:24b6f81f1983e6df8db3adc38562c83f7d4a0c36162885ec7f7b77c7dcbec97b \ - --hash=sha256:256f80b80ca3853f90c21b23ee78cd008713787b1b1e93eae9f3d6a7134abd91 \ - --hash=sha256:28a3a209b96630bca57cce802da70c266eb08c6e97e5afd61a75611ee6c64592 \ - --hash=sha256:2c8f814d84194c9ea681642fd164267891702542f028a15fc97d4674b6206187 \ - --hash=sha256:2de9a304e27f7596cd03d16f1b7c72219bd944e99cc52b84d0145aefb07cbd3c \ - --hash=sha256:38100abb9d1b1435bc4cc340bb4489635dc2f0da7456590877030c9b3d40b0c1 \ - --hash=sha256:3925dd22fa2b7699ed2617149842d2e6adde22b262fcbfada50e3d195e4b3a94 \ - --hash=sha256:3e17ed538242334bf70832644a32a7aae3d83b57567f9fd60a26257e992b79ba \ - --hash=sha256:3e837e369566884707ddaf85fc1744b47575005c0a229de3327f8f9a20f4efeb \ - --hash=sha256:3f4d46d8b35698056ec29bca21546e1551a205058ae1a181d871e278b0b28165 \ - --hash=sha256:44d1b5909021139fe36001ae048dbdde8214afa20200eda0f64c068cac5d5529 \ - --hash=sha256:45d5e886156860dc35862657e1494b9bae8dfa63bf56796f2fb56e1679fc0bca \ - --hash=sha256:4647afc2f90d1ddd33441e5b0e85b16b12ddec4fca55f0d9671fef036ecca27c \ - --hash=sha256:4671d9dd5ec934cb9a73e7ee9676f9362aba54f7f34910956b84d727b0d73fb6 \ - --hash=sha256:53f77cbe57044e88bbd5ed26ac1d0514d2acf0591dd6bb02a3ae37f76811b80c \ - --hash=sha256:5eda85d6d1879e692d546a078b44251cdd08dd1cfb98dfb77b670c97cee49ea0 \ - --hash=sha256:5fed36fccc0612a53f1d4d9a816b50a36702c28a2aa880cb8a122b3466638743 \ - --hash=sha256:61d028e90346df14fedc3d1e5441df818d095f3b87d286825dfcbd6459b7ef63 \ - --hash=sha256:66f011380d0e49ed280c789fbd08ff0d40968ee7b665575489afa95c98196ab5 \ - --hash=sha256:6824f87845e3396029f3820c206e459ccc91760e8fa24422f8b0c3d1731cbec5 \ - --hash=sha256:6c6c373cfc5c83a975506110d17457138c8c63016b563cc9ed6e056a82f13ce4 \ - --hash=sha256:6d02d6655b0e54f54c4ef0b94eb6be0607b70853c45ce98bd278dc7de718be5d \ - --hash=sha256:6d50360be4546678fc1b79ffe7a66265e28667840010348dd69a314145807a1b \ - --hash=sha256:730cacb21e1bdff3ce90babf007d0a0917cc3e6492f336c2f0134101e0944f93 \ - --hash=sha256:737fe7d37e1a1bffe70bd5754ea763a62a066dc5913ca57e957824b72a85e205 \ - --hash=sha256:74a03b9698e198d47562765773b4a8309919089150a0bb17d829ad7b44b60d27 \ - --hash=sha256:7553fb2090d71822f02c629afe6042c299edf91ba1bf94951165613553984512 \ - --hash=sha256:7a66c7204d8869299919db4d5069a82f1561581af12b11b3c9f48c584eb8743d \ - --hash=sha256:7cc09976e8b56f8cebd752f7113ad07752461f48a58cbba644139015ac24954c \ - --hash=sha256:81afed14892743bbe14dacb9e36d9e0e504cd204e0b165062c488942b9718037 \ - --hash=sha256:8941aaadaf67246224cee8c3803777eed332a19d909b47e29c9842ef1e79ac26 \ - --hash=sha256:89472c9762729b5ae1ad974b777416bfda4ac5642423fa93bd57a09204712322 \ - --hash=sha256:8ea985900c5c95ce9db1745f7933eeef5d314f0565b27625d9a10ec9881e1bfb \ - --hash=sha256:8eca2a813c1cb7ad4fb74d368c2ffbbb4789d377ee5bb8df98373c2cc0dee76c \ - --hash=sha256:92b68146a71df78564e4ef48af17551a5ddd142e5190cdf2c5624d0c3ff5b2e8 \ - --hash=sha256:9332088d75dc3241c702d852d4671613136d90fa6881da7d770a483fd05248b4 \ - --hash=sha256:94698a9c5f91f9d138526b48fe26a199609544591f859c870d477351dc7b2414 \ - --hash=sha256:9a67fc9e8eb39039280526379fb3a70023d77caec1852002b4da7e8b270c4dd9 \ - --hash=sha256:9de40a7b0323d889cf8d23d1ef214f565ab154443c42737dfe52ff82cf857664 \ - --hash=sha256:a05d0c237b3349096d3981b727493e22147f934b20f6f125a3eba8f994bec4a9 \ - --hash=sha256:afb8db5439b81cf9c9d0c80404b60c3cc9c3add93e114dcae767f1477cb53775 \ - --hash=sha256:b18a3ed7d5b3bd8d9ef7a8cb226502c6bf8308df1525e1cc676c3680e7176739 \ - --hash=sha256:b1e74d11748e7e98e2f426ab176d4ed720a64412b6a15054378afdb71e0f37dc \ - --hash=sha256:b21e08af67b8a103c71a250401c78d5e0893beff75e28c53c98f4de42f774062 \ - --hash=sha256:b4c854ef3adc177950a8dfc81a86f5115d2abd545751a304c5bcf2c2c7283cfe \ - --hash=sha256:b882b3df248017dba09d6b16defe9b5c407fe32fc7c65a9c69798e6175601be9 \ - --hash=sha256:baf5215e0ab74c16e2dd324e8ec067ef59e41125d3eade2b863d294fd5035c92 \ - --hash=sha256:c649e3a33450ec82378822b3dad03cc228b8f5963c0c12fc3b1e0ab940f768a5 \ - --hash=sha256:c654de545946e0db659b3400168c9ad31b5d29593291482c43e3564effbcee13 \ - --hash=sha256:c6638687455baf640e37344fe26d37c404db8b80d037c3d29f58fe8d1c3b194d \ - --hash=sha256:c8d3b5532fc71b7a77c09192b4a5a200ea992702734a2e9279a37f2478236f26 \ - --hash=sha256:cb527a79772e5ef98fb1d700678fe031e353e765d1ca2d409c92263c6d43e09f \ - --hash=sha256:cf364028c016c03078a23b503f02058f1814320a56ad535686f90565636a9495 \ - --hash=sha256:d48a880098c96020b02d5a1f7d9251308510ce8858940e6fa99ece33f610838b \ - --hash=sha256:d68b6cef7827e8641e8ef16f4494edda8b36104d79773a334beaa1e3521430f6 \ - --hash=sha256:d9b29c1f0ae438d5ee9acb31cadee00a58c46cc9c0b2f9038c6b0b3470877a8c \ - --hash=sha256:d9b97165e8aed9272a6bb17c01e3cc5871a594a446ebedc996e2397a1c1ea8ef \ - --hash=sha256:da68248800ad6320861f129cd9c1bf96ca849a2771a59e0344e88681905916f5 \ - --hash=sha256:da902562c3e9c550df360bfa53c035b2f241fed6d9aef119048073680ace4a18 \ - --hash=sha256:dbd5c7a25a7cb98f5ca55d258b103a2054f859a46ae11aaf23134f9cc0d356ad \ - --hash=sha256:dd4f05f54a52fb558f1ba9f528228066954fee3ebe629fc1660d874d040ae5a3 \ - --hash=sha256:de8dad4425a6ca6e4e5e297b27b5c824ecc7581910bf9aee86cb6835e6812aa7 \ - --hash=sha256:e11e82b744887154b182fd3e7e8512418446501191994dbf9c9fc1f32cc8efd5 \ - --hash=sha256:e6e73b9e02893c764e7e8d5bb5ce277f1a009cd5243f8228f75f842bf937c534 \ - --hash=sha256:f73b96c41e3b2adedc34a7356e64c8eb96e03a3782b535e043a986276ce12a49 \ - --hash=sha256:f93fd8e5c8c0a4aa1f424d6173f14a892044054871c771f8566e4008eaa359d2 \ - --hash=sha256:fc33c5141b55ed366cfaad382df24fe7dcbc686de5be719b207bb248e3053dc5 \ - --hash=sha256:fc7de24befaeae77ba923797c7c87834c73648a05a4bde34b3b7e5588973a453 \ - --hash=sha256:fe562eb1a64e67dd297ccc4f5addea2501664954f2692b69a76449ec7913ecbf - # via cryptography -cryptography==46.0.1 \ - --hash=sha256:0a17377fa52563d730248ba1f68185461fff36e8bc75d8787a7dd2e20a802b7a \ - --hash=sha256:0ca4be2af48c24df689a150d9cd37404f689e2968e247b6b8ff09bff5bcd786f \ - --hash=sha256:0d1922d9280e08cde90b518a10cd66831f632960a8d08cb3418922d83fce6f12 \ - --hash=sha256:0dfb7c88d4462a0cfdd0d87a3c245a7bc3feb59de101f6ff88194f740f72eda6 \ - --hash=sha256:0ff483716be32690c14636e54a1f6e2e1b7bf8e22ca50b989f88fa1b2d287080 \ - --hash=sha256:13e67c4d3fb8b6bc4ef778a7ccdd8df4cd15b4bcc18f4239c8440891a11245cc \ - --hash=sha256:15b5fd9358803b0d1cc42505a18d8bca81dabb35b5cfbfea1505092e13a9d96d \ - --hash=sha256:1cd6d50c1a8b79af1a6f703709d8973845f677c8e97b1268f5ff323d38ce8475 \ - --hash=sha256:2dd339ba3345b908fa3141ddba4025568fa6fd398eabce3ef72a29ac2d73ad75 \ - --hash=sha256:341fb7a26bc9d6093c1b124b9f13acc283d2d51da440b98b55ab3f79f2522ead \ - --hash=sha256:34f04b7311174469ab3ac2647469743720f8b6c8b046f238e5cb27905695eb2a \ - --hash=sha256:41c281a74df173876da1dc9a9b6953d387f06e3d3ed9284e3baae3ab3f40883a \ - --hash=sha256:449ef2b321bec7d97ef2c944173275ebdab78f3abdd005400cc409e27cd159ab \ - --hash=sha256:45f790934ac1018adeba46a0f7289b2b8fe76ba774a88c7f1922213a56c98bc1 \ - --hash=sha256:48948940d0ae00483e85e9154bb42997d0b77c21e43a77b7773c8c80de532ac5 \ - --hash=sha256:4c49eda9a23019e11d32a0eb51a27b3e7ddedde91e099c0ac6373e3aacc0d2ee \ - --hash=sha256:504e464944f2c003a0785b81668fe23c06f3b037e9cb9f68a7c672246319f277 \ - --hash=sha256:534b96c0831855e29fc3b069b085fd185aa5353033631a585d5cd4dd5d40d657 \ - --hash=sha256:6ef1488967e729948d424d09c94753d0167ce59afba8d0f6c07a22b629c557b2 \ - --hash=sha256:7176a5ab56fac98d706921f6416a05e5aff7df0e4b91516f450f8627cda22af3 \ - --hash=sha256:7411c910fb2a412053cf33cfad0153ee20d27e256c6c3f14d7d7d1d9fec59fd5 \ - --hash=sha256:757af4f6341ce7a1e47c326ca2a81f41d236070217e5fbbad61bbfe299d55d28 \ - --hash=sha256:7823bc7cdf0b747ecfb096d004cc41573c2f5c7e3a29861603a2871b43d3ef32 \ - --hash=sha256:7fab1187b6c6b2f11a326f33b036f7168f5b996aedd0c059f9738915e4e8f53a \ - --hash=sha256:84ef1f145de5aee82ea2447224dc23f065ff4cc5791bb3b506615957a6ba8128 \ - --hash=sha256:92e8cfe8bd7dd86eac0a677499894862cd5cc2fd74de917daa881d00871ac8e7 \ - --hash=sha256:9394c7d5a7565ac5f7d9ba38b2617448eba384d7b107b262d63890079fad77ca \ - --hash=sha256:9495d78f52c804b5ec8878b5b8c7873aa8e63db9cd9ee387ff2db3fffe4df784 \ - --hash=sha256:9873bf7c1f2a6330bdfe8621e7ce64b725784f9f0c3a6a55c3047af5849f920e \ - --hash=sha256:9babb7818fdd71394e576cf26c5452df77a355eac1a27ddfa24096665a27f8fd \ - --hash=sha256:9e8776dac9e660c22241b6587fae51a67b4b0147daa4d176b172c3ff768ad736 \ - --hash=sha256:9ed64e5083fa806709e74fc5ea067dfef9090e5b7a2320a49be3c9df3583a2d8 \ - --hash=sha256:9f2c4cc63be3ef43c0221861177cee5d14b505cd4d4599a89e2cd273c4d3542a \ - --hash=sha256:9f40642a140c0c8649987027867242b801486865277cbabc8c6059ddef16dc8b \ - --hash=sha256:af84e8e99f1a82cea149e253014ea9dc89f75b82c87bb6c7242203186f465129 \ - --hash=sha256:b9c79af2c3058430d911ff1a5b2b96bbfe8da47d5ed961639ce4681886614e70 \ - --hash=sha256:c52fded6383f7e20eaf70a60aeddd796b3677c3ad2922c801be330db62778e05 \ - --hash=sha256:cbb8e769d4cac884bb28e3ff620ef1001b75588a5c83c9c9f1fdc9afbe7f29b0 \ - --hash=sha256:d84c40bdb8674c29fa192373498b6cb1e84f882889d21a471b45d1f868d8d44b \ - --hash=sha256:db5597a4c7353b2e5fb05a8e6cb74b56a4658a2b7bf3cb6b1821ae7e7fd6eaa0 \ - --hash=sha256:e22801b61613ebdebf7deb18b507919e107547a1d39a3b57f5f855032dd7cfb8 \ - --hash=sha256:e34da95e29daf8a71cb2841fd55df0511539a6cdf33e6f77c1e95e44006b9b46 \ - --hash=sha256:e46710a240a41d594953012213ea8ca398cd2448fbc5d0f1be8160b5511104a0 \ - --hash=sha256:e94eb5fa32a8a9f9bf991f424f002913e3dd7c699ef552db9b14ba6a76a6313b \ - --hash=sha256:ec13b7105117dbc9afd023300fb9954d72ca855c274fe563e72428ece10191c0 \ - --hash=sha256:ed570874e88f213437f5cf758f9ef26cbfc3f336d889b1e592ee11283bb8d1c7 \ - --hash=sha256:ed957044e368ed295257ae3d212b95456bd9756df490e1ac4538857f67531fcc \ - --hash=sha256:ef648d2c690703501714588b2ba640facd50fd16548133b11b2859e8655a69da \ - --hash=sha256:efc9e51c3e595267ff84adf56e9b357db89ab2279d7e375ffcaf8f678606f3d9 \ - --hash=sha256:f736ab8036796f5a119ff8211deda416f8c15ce03776db704a7a4e17381cb2ef \ - --hash=sha256:f7a24ea78de345cfa7f6a8d3bde8b242c7fac27f2bd78fa23474ca38dfaeeab9 \ - --hash=sha256:f7de12fa0eee6234de9a9ce0ffcfa6ce97361db7a50b09b65c63ac58e5f22fc7 \ - --hash=sha256:f9b55038b5c6c47559aa33626d8ecd092f354e23de3c6975e4bb205df128a2a0 \ - --hash=sha256:fd4b5e2ee4e60425711ec65c33add4e7a626adef79d66f62ba0acfd493af282d - # via ansible-core -jinja2==3.1.6 \ - --hash=sha256:0137fb05990d35f1275a587e9aee6d56da821fc83491a0fb838183be43f66d6d \ - --hash=sha256:85ece4451f492d0c13c5dd7c13a64681a86afae63a5f347908daf103ce6d2f67 - # via ansible-core -markupsafe==3.0.3 \ - --hash=sha256:0303439a41979d9e74d18ff5e2dd8c43ed6c6001fd40e5bf2e43f7bd9bbc523f \ - --hash=sha256:068f375c472b3e7acbe2d5318dea141359e6900156b5b2ba06a30b169086b91a \ - --hash=sha256:0bf2a864d67e76e5c9a34dc26ec616a66b9888e25e7b9460e1c76d3293bd9dbf \ - --hash=sha256:0db14f5dafddbb6d9208827849fad01f1a2609380add406671a26386cdf15a19 \ - --hash=sha256:0eb9ff8191e8498cca014656ae6b8d61f39da5f95b488805da4bb029cccbfbaf \ - --hash=sha256:0f4b68347f8c5eab4a13419215bdfd7f8c9b19f2b25520968adfad23eb0ce60c \ - --hash=sha256:1085e7fbddd3be5f89cc898938f42c0b3c711fdcb37d75221de2666af647c175 \ - --hash=sha256:116bb52f642a37c115f517494ea5feb03889e04df47eeff5b130b1808ce7c219 \ - --hash=sha256:12c63dfb4a98206f045aa9563db46507995f7ef6d83b2f68eda65c307c6829eb \ - --hash=sha256:133a43e73a802c5562be9bbcd03d090aa5a1fe899db609c29e8c8d815c5f6de6 \ - --hash=sha256:1353ef0c1b138e1907ae78e2f6c63ff67501122006b0f9abad68fda5f4ffc6ab \ - --hash=sha256:15d939a21d546304880945ca1ecb8a039db6b4dc49b2c5a400387cdae6a62e26 \ - --hash=sha256:177b5253b2834fe3678cb4a5f0059808258584c559193998be2601324fdeafb1 \ - --hash=sha256:1872df69a4de6aead3491198eaf13810b565bdbeec3ae2dc8780f14458ec73ce \ - --hash=sha256:1b4b79e8ebf6b55351f0d91fe80f893b4743f104bff22e90697db1590e47a218 \ - --hash=sha256:1b52b4fb9df4eb9ae465f8d0c228a00624de2334f216f178a995ccdcf82c4634 \ - --hash=sha256:1ba88449deb3de88bd40044603fafffb7bc2b055d626a330323a9ed736661695 \ - --hash=sha256:1cc7ea17a6824959616c525620e387f6dd30fec8cb44f649e31712db02123dad \ - --hash=sha256:218551f6df4868a8d527e3062d0fb968682fe92054e89978594c28e642c43a73 \ - --hash=sha256:26a5784ded40c9e318cfc2bdb30fe164bdb8665ded9cd64d500a34fb42067b1c \ - --hash=sha256:2713baf880df847f2bece4230d4d094280f4e67b1e813eec43b4c0e144a34ffe \ - --hash=sha256:2a15a08b17dd94c53a1da0438822d70ebcd13f8c3a95abe3a9ef9f11a94830aa \ - --hash=sha256:2f981d352f04553a7171b8e44369f2af4055f888dfb147d55e42d29e29e74559 \ - --hash=sha256:32001d6a8fc98c8cb5c947787c5d08b0a50663d139f1305bac5885d98d9b40fa \ - --hash=sha256:3524b778fe5cfb3452a09d31e7b5adefeea8c5be1d43c4f810ba09f2ceb29d37 \ - --hash=sha256:3537e01efc9d4dccdf77221fb1cb3b8e1a38d5428920e0657ce299b20324d758 \ - --hash=sha256:35add3b638a5d900e807944a078b51922212fb3dedb01633a8defc4b01a3c85f \ - --hash=sha256:38664109c14ffc9e7437e86b4dceb442b0096dfe3541d7864d9cbe1da4cf36c8 \ - --hash=sha256:3a7e8ae81ae39e62a41ec302f972ba6ae23a5c5396c8e60113e9066ef893da0d \ - --hash=sha256:3b562dd9e9ea93f13d53989d23a7e775fdfd1066c33494ff43f5418bc8c58a5c \ - --hash=sha256:457a69a9577064c05a97c41f4e65148652db078a3a509039e64d3467b9e7ef97 \ - --hash=sha256:4bd4cd07944443f5a265608cc6aab442e4f74dff8088b0dfc8238647b8f6ae9a \ - --hash=sha256:4e885a3d1efa2eadc93c894a21770e4bc67899e3543680313b09f139e149ab19 \ - --hash=sha256:4faffd047e07c38848ce017e8725090413cd80cbc23d86e55c587bf979e579c9 \ - --hash=sha256:509fa21c6deb7a7a273d629cf5ec029bc209d1a51178615ddf718f5918992ab9 \ - --hash=sha256:5678211cb9333a6468fb8d8be0305520aa073f50d17f089b5b4b477ea6e67fdc \ - --hash=sha256:591ae9f2a647529ca990bc681daebdd52c8791ff06c2bfa05b65163e28102ef2 \ - --hash=sha256:5a7d5dc5140555cf21a6fefbdbf8723f06fcd2f63ef108f2854de715e4422cb4 \ - --hash=sha256:69c0b73548bc525c8cb9a251cddf1931d1db4d2258e9599c28c07ef3580ef354 \ - --hash=sha256:6b5420a1d9450023228968e7e6a9ce57f65d148ab56d2313fcd589eee96a7a50 \ - --hash=sha256:722695808f4b6457b320fdc131280796bdceb04ab50fe1795cd540799ebe1698 \ - --hash=sha256:729586769a26dbceff69f7a7dbbf59ab6572b99d94576a5592625d5b411576b9 \ - --hash=sha256:77f0643abe7495da77fb436f50f8dab76dbc6e5fd25d39589a0f1fe6548bfa2b \ - --hash=sha256:795e7751525cae078558e679d646ae45574b47ed6e7771863fcc079a6171a0fc \ - --hash=sha256:7be7b61bb172e1ed687f1754f8e7484f1c8019780f6f6b0786e76bb01c2ae115 \ - --hash=sha256:7c3fb7d25180895632e5d3148dbdc29ea38ccb7fd210aa27acbd1201a1902c6e \ - --hash=sha256:7e68f88e5b8799aa49c85cd116c932a1ac15caaa3f5db09087854d218359e485 \ - --hash=sha256:83891d0e9fb81a825d9a6d61e3f07550ca70a076484292a70fde82c4b807286f \ - --hash=sha256:8485f406a96febb5140bfeca44a73e3ce5116b2501ac54fe953e488fb1d03b12 \ - --hash=sha256:8709b08f4a89aa7586de0aadc8da56180242ee0ada3999749b183aa23df95025 \ - --hash=sha256:8f71bc33915be5186016f675cd83a1e08523649b0e33efdb898db577ef5bb009 \ - --hash=sha256:915c04ba3851909ce68ccc2b8e2cd691618c4dc4c4232fb7982bca3f41fd8c3d \ - --hash=sha256:949b8d66bc381ee8b007cd945914c721d9aba8e27f71959d750a46f7c282b20b \ - --hash=sha256:94c6f0bb423f739146aec64595853541634bde58b2135f27f61c1ffd1cd4d16a \ - --hash=sha256:9a1abfdc021a164803f4d485104931fb8f8c1efd55bc6b748d2f5774e78b62c5 \ - --hash=sha256:9b79b7a16f7fedff2495d684f2b59b0457c3b493778c9eed31111be64d58279f \ - --hash=sha256:a320721ab5a1aba0a233739394eb907f8c8da5c98c9181d1161e77a0c8e36f2d \ - --hash=sha256:a4afe79fb3de0b7097d81da19090f4df4f8d3a2b3adaa8764138aac2e44f3af1 \ - --hash=sha256:ad2cf8aa28b8c020ab2fc8287b0f823d0a7d8630784c31e9ee5edea20f406287 \ - --hash=sha256:b8512a91625c9b3da6f127803b166b629725e68af71f8184ae7e7d54686a56d6 \ - --hash=sha256:bc51efed119bc9cfdf792cdeaa4d67e8f6fcccab66ed4bfdd6bde3e59bfcbb2f \ - --hash=sha256:bdc919ead48f234740ad807933cdf545180bfbe9342c2bb451556db2ed958581 \ - --hash=sha256:bdd37121970bfd8be76c5fb069c7751683bdf373db1ed6c010162b2a130248ed \ - --hash=sha256:be8813b57049a7dc738189df53d69395eba14fb99345e0a5994914a3864c8a4b \ - --hash=sha256:c0c0b3ade1c0b13b936d7970b1d37a57acde9199dc2aecc4c336773e1d86049c \ - --hash=sha256:c47a551199eb8eb2121d4f0f15ae0f923d31350ab9280078d1e5f12b249e0026 \ - --hash=sha256:c4ffb7ebf07cfe8931028e3e4c85f0357459a3f9f9490886198848f4fa002ec8 \ - --hash=sha256:ccfcd093f13f0f0b7fdd0f198b90053bf7b2f02a3927a30e63f3ccc9df56b676 \ - --hash=sha256:d2ee202e79d8ed691ceebae8e0486bd9a2cd4794cec4824e1c99b6f5009502f6 \ - --hash=sha256:d53197da72cc091b024dd97249dfc7794d6a56530370992a5e1a08983ad9230e \ - --hash=sha256:d6dd0be5b5b189d31db7cda48b91d7e0a9795f31430b7f271219ab30f1d3ac9d \ - --hash=sha256:d88b440e37a16e651bda4c7c2b930eb586fd15ca7406cb39e211fcff3bf3017d \ - --hash=sha256:de8a88e63464af587c950061a5e6a67d3632e36df62b986892331d4620a35c01 \ - --hash=sha256:df2449253ef108a379b8b5d6b43f4b1a8e81a061d6537becd5582fba5f9196d7 \ - --hash=sha256:e1c1493fb6e50ab01d20a22826e57520f1284df32f2d8601fdd90b6304601419 \ - --hash=sha256:e1cf1972137e83c5d4c136c43ced9ac51d0e124706ee1c8aa8532c1287fa8795 \ - --hash=sha256:e2103a929dfa2fcaf9bb4e7c091983a49c9ac3b19c9061b6d5427dd7d14d81a1 \ - --hash=sha256:e56b7d45a839a697b5eb268c82a71bd8c7f6c94d6fd50c3d577fa39a9f1409f5 \ - --hash=sha256:e8afc3f2ccfa24215f8cb28dcf43f0113ac3c37c2f0f0806d8c70e4228c5cf4d \ - --hash=sha256:e8fc20152abba6b83724d7ff268c249fa196d8259ff481f3b1476383f8f24e42 \ - --hash=sha256:eaa9599de571d72e2daf60164784109f19978b327a3910d3e9de8c97b5b70cfe \ - --hash=sha256:ec15a59cf5af7be74194f7ab02d0f59a62bdcf1a537677ce67a2537c9b87fcda \ - --hash=sha256:f190daf01f13c72eac4efd5c430a8de82489d9cff23c364c3ea822545032993e \ - --hash=sha256:f34c41761022dd093b4b6896d4810782ffbabe30f2d443ff5f083e0cbbb8c737 \ - --hash=sha256:f3e98bb3798ead92273dc0e5fd0f31ade220f59a266ffd8a4f6065e0a3ce0523 \ - --hash=sha256:f42d0984e947b8adf7dd6dde396e720934d12c506ce84eea8476409563607591 \ - --hash=sha256:f71a396b3bf33ecaa1626c255855702aca4d3d9fea5e051b41ac59a9c1c41edc \ - --hash=sha256:f9e130248f4462aaa8e2552d547f36ddadbeaa573879158d721bbd33dfe4743a \ - --hash=sha256:fed51ac40f757d41b7c48425901843666a6677e3e8eb0abcff09e4ba6e664f50 - # via jinja2 -packaging==25.0 \ - --hash=sha256:29572ef2b1f17581046b3a2227d5c611fb25ec70ca1ba8554b24b0e69331a484 \ - --hash=sha256:d443872c98d677bf60f6a1f2f8c1cb748e8fe762d2bf9d3148b5599295b0fc4f - # via ansible-core -pycparser==2.23 \ - --hash=sha256:78816d4f24add8f10a06d6f05b4d424ad9e96cfebf68a4ddc99c65c0720d00c2 \ - --hash=sha256:e5c6e8d3fbad53479cab09ac03729e0a9faf2bee3db8208a550daf5af81a5934 - # via cffi -pyyaml==6.0.3 \ - --hash=sha256:00c4bdeba853cc34e7dd471f16b4114f4162dc03e6b7afcc2128711f0eca823c \ - --hash=sha256:0150219816b6a1fa26fb4699fb7daa9caf09eb1999f3b70fb6e786805e80375a \ - --hash=sha256:02893d100e99e03eda1c8fd5c441d8c60103fd175728e23e431db1b589cf5ab3 \ - --hash=sha256:02ea2dfa234451bbb8772601d7b8e426c2bfa197136796224e50e35a78777956 \ - --hash=sha256:0f29edc409a6392443abf94b9cf89ce99889a1dd5376d94316ae5145dfedd5d6 \ - --hash=sha256:10892704fc220243f5305762e276552a0395f7beb4dbf9b14ec8fd43b57f126c \ - --hash=sha256:16249ee61e95f858e83976573de0f5b2893b3677ba71c9dd36b9cf8be9ac6d65 \ - --hash=sha256:1d37d57ad971609cf3c53ba6a7e365e40660e3be0e5175fa9f2365a379d6095a \ - --hash=sha256:1ebe39cb5fc479422b83de611d14e2c0d3bb2a18bbcb01f229ab3cfbd8fee7a0 \ - --hash=sha256:214ed4befebe12df36bcc8bc2b64b396ca31be9304b8f59e25c11cf94a4c033b \ - --hash=sha256:2283a07e2c21a2aa78d9c4442724ec1eb15f5e42a723b99cb3d822d48f5f7ad1 \ - --hash=sha256:27c0abcb4a5dac13684a37f76e701e054692a9b2d3064b70f5e4eb54810553d7 \ - --hash=sha256:28c8d926f98f432f88adc23edf2e6d4921ac26fb084b028c733d01868d19007e \ - --hash=sha256:2e71d11abed7344e42a8849600193d15b6def118602c4c176f748e4583246007 \ - --hash=sha256:34d5fcd24b8445fadc33f9cf348c1047101756fd760b4dacb5c3e99755703310 \ - --hash=sha256:37503bfbfc9d2c40b344d06b2199cf0e96e97957ab1c1b546fd4f87e53e5d3e4 \ - --hash=sha256:3c5677e12444c15717b902a5798264fa7909e41153cdf9ef7ad571b704a63dd9 \ - --hash=sha256:41715c910c881bc081f1e8872880d3c650acf13dfa8214bad49ed4cede7c34ea \ - --hash=sha256:418cf3f2111bc80e0933b2cd8cd04f286338bb88bdc7bc8e6dd775ebde60b5e0 \ - --hash=sha256:44edc647873928551a01e7a563d7452ccdebee747728c1080d881d68af7b997e \ - --hash=sha256:4a2e8cebe2ff6ab7d1050ecd59c25d4c8bd7e6f400f5f82b96557ac0abafd0ac \ - --hash=sha256:4ad1906908f2f5ae4e5a8ddfce73c320c2a1429ec52eafd27138b7f1cbe341c9 \ - --hash=sha256:501a031947e3a9025ed4405a168e6ef5ae3126c59f90ce0cd6f2bfc477be31b7 \ - --hash=sha256:5190d403f121660ce8d1d2c1bb2ef1bd05b5f68533fc5c2ea899bd15f4399b35 \ - --hash=sha256:5498cd1645aa724a7c71c8f378eb29ebe23da2fc0d7a08071d89469bf1d2defb \ - --hash=sha256:5e0b74767e5f8c593e8c9b5912019159ed0533c70051e9cce3e8b6aa699fcd69 \ - --hash=sha256:5ed875a24292240029e4483f9d4a4b8a1ae08843b9c54f43fcc11e404532a8a5 \ - --hash=sha256:5fcd34e47f6e0b794d17de1b4ff496c00986e1c83f7ab2fb8fcfe9616ff7477b \ - --hash=sha256:5fdec68f91a0c6739b380c83b951e2c72ac0197ace422360e6d5a959d8d97b2c \ - --hash=sha256:64386e5e707d03a7e172c0701abfb7e10f0fb753ee1d773128192742712a98fd \ - --hash=sha256:652cb6edd41e718550aad172851962662ff2681490a8a711af6a4d288dd96824 \ - --hash=sha256:66291b10affd76d76f54fad28e22e51719ef9ba22b29e1d7d03d6777a9174198 \ - --hash=sha256:66e1674c3ef6f541c35191caae2d429b967b99e02040f5ba928632d9a7f0f065 \ - --hash=sha256:6adc77889b628398debc7b65c073bcb99c4a0237b248cacaf3fe8a557563ef6c \ - --hash=sha256:79005a0d97d5ddabfeeea4cf676af11e647e41d81c9a7722a193022accdb6b7c \ - --hash=sha256:7c6610def4f163542a622a73fb39f534f8c101d690126992300bf3207eab9764 \ - --hash=sha256:7f047e29dcae44602496db43be01ad42fc6f1cc0d8cd6c83d342306c32270196 \ - --hash=sha256:8098f252adfa6c80ab48096053f512f2321f0b998f98150cea9bd23d83e1467b \ - --hash=sha256:850774a7879607d3a6f50d36d04f00ee69e7fc816450e5f7e58d7f17f1ae5c00 \ - --hash=sha256:8d1fab6bb153a416f9aeb4b8763bc0f22a5586065f86f7664fc23339fc1c1fac \ - --hash=sha256:8da9669d359f02c0b91ccc01cac4a67f16afec0dac22c2ad09f46bee0697eba8 \ - --hash=sha256:8dc52c23056b9ddd46818a57b78404882310fb473d63f17b07d5c40421e47f8e \ - --hash=sha256:9149cad251584d5fb4981be1ecde53a1ca46c891a79788c0df828d2f166bda28 \ - --hash=sha256:93dda82c9c22deb0a405ea4dc5f2d0cda384168e466364dec6255b293923b2f3 \ - --hash=sha256:96b533f0e99f6579b3d4d4995707cf36df9100d67e0c8303a0c55b27b5f99bc5 \ - --hash=sha256:9c7708761fccb9397fe64bbc0395abcae8c4bf7b0eac081e12b809bf47700d0b \ - --hash=sha256:9f3bfb4965eb874431221a3ff3fdcddc7e74e3b07799e0e84ca4a0f867d449bf \ - --hash=sha256:a33284e20b78bd4a18c8c2282d549d10bc8408a2a7ff57653c0cf0b9be0afce5 \ - --hash=sha256:a80cb027f6b349846a3bf6d73b5e95e782175e52f22108cfa17876aaeff93702 \ - --hash=sha256:b30236e45cf30d2b8e7b3e85881719e98507abed1011bf463a8fa23e9c3e98a8 \ - --hash=sha256:b3bc83488de33889877a0f2543ade9f70c67d66d9ebb4ac959502e12de895788 \ - --hash=sha256:b865addae83924361678b652338317d1bd7e79b1f4596f96b96c77a5a34b34da \ - --hash=sha256:b8bb0864c5a28024fac8a632c443c87c5aa6f215c0b126c449ae1a150412f31d \ - --hash=sha256:ba1cc08a7ccde2d2ec775841541641e4548226580ab850948cbfda66a1befcdc \ - --hash=sha256:bdb2c67c6c1390b63c6ff89f210c8fd09d9a1217a465701eac7316313c915e4c \ - --hash=sha256:c1ff362665ae507275af2853520967820d9124984e0f7466736aea23d8611fba \ - --hash=sha256:c3355370a2c156cffb25e876646f149d5d68f5e0a3ce86a5084dd0b64a994917 \ - --hash=sha256:c458b6d084f9b935061bc36216e8a69a7e293a2f1e68bf956dcd9e6cbcd143f5 \ - --hash=sha256:d0eae10f8159e8fdad514efdc92d74fd8d682c933a6dd088030f3834bc8e6b26 \ - --hash=sha256:d76623373421df22fb4cf8817020cbb7ef15c725b9d5e45f17e189bfc384190f \ - --hash=sha256:ebc55a14a21cb14062aa4162f906cd962b28e2e9ea38f9b4391244cd8de4ae0b \ - --hash=sha256:eda16858a3cab07b80edaf74336ece1f986ba330fdb8ee0d6c0d68fe82bc96be \ - --hash=sha256:ee2922902c45ae8ccada2c5b501ab86c36525b883eff4255313a253a3160861c \ - --hash=sha256:f7057c9a337546edc7973c0d3ba84ddcdf0daa14533c2065749c9075001090e6 \ - --hash=sha256:fa160448684b4e94d80416c0fa4aac48967a969efe22931448d853ada8baf926 \ - --hash=sha256:fc09d0aa354569bc501d4e787133afc08552722d3ab34836a80547331bb5d4a0 - # via ansible-core -resolvelib==1.0.1 \ - --hash=sha256:04ce76cbd63fded2078ce224785da6ecd42b9564b1390793f64ddecbe997b309 \ - --hash=sha256:d2da45d1a8dfee81bdd591647783e340ef3bcb104b54c383f70d422ef5cc7dbf - # via ansible-core diff --git a/src/software/power/powerloop_main.cc b/src/software/power/powerloop_main.cc index 81042cb912..102ecbb707 100644 --- a/src/software/power/powerloop_main.cc +++ b/src/software/power/powerloop_main.cc @@ -104,13 +104,17 @@ void loop() monitor->getCurrentDrawAmp(), geneva->getCurrentSlot(), sequence_num++, chicker->getBreakBeamTripped()); - if (dribble_target <= dribbler_speed) { + if (dribble_target <= dribbler_speed) + { dribbler_speed = dribble_target; - } else { + } + else + { // Ramp to speed - dribbler_speed = dribbler_speed + (dribble_target-dribbler_speed)/RAMP_FACTOR + 1; + dribbler_speed = + dribbler_speed + (dribble_target - dribbler_speed) / RAMP_FACTOR + 1; } - dribbler->dribble(dribbler_speed/DRIBBLER_MAX_SPEED*255); + dribbler->dribble(dribbler_speed / DRIBBLER_MAX_SPEED * 255); // Write sensor values out to Serial TbotsProto_PowerFrame status_frame = createUartFrame(status); From 428e93bda63e8de5a225368760626e1c3f64688d Mon Sep 17 00:00:00 2001 From: Grayson Date: Thu, 4 Jun 2026 23:40:36 -0700 Subject: [PATCH 31/34] tests and compilation --- src/software/embedded/motor_controller/BUILD | 1 + .../stspin_motor_controller.cpp | 7 +- .../stspin_motor_controller.h | 6 +- .../stspin_motor_controller_test.cpp | 5 +- src/software/embedded/services/BUILD | 21 ++- src/software/embedded/services/power.cpp | 133 +----------------- src/software/embedded/services/power.h | 59 +------- .../embedded/services/uart_communicator.cpp | 102 ++++++++++++++ .../embedded/services/uart_communicator.h | 71 ++++++++++ src/software/embedded/thunderloop.cpp | 25 ++-- 10 files changed, 216 insertions(+), 214 deletions(-) create mode 100644 src/software/embedded/services/uart_communicator.cpp create mode 100644 src/software/embedded/services/uart_communicator.h diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index 588398fb60..8e1b1aa90c 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -69,6 +69,7 @@ cc_binary( ":motor_controller", "//software/logger", "//software/logger:network_logger", + "//software/embedded/services:uart_communicator", "@boost//:program_options", ], ) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 7e92d86fbc..67e967b089 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -22,12 +22,11 @@ using Crc8Autosar = crc_utils::crc; StSpinMotorController::StSpinMotorController( - const robot_constants::RobotConstants& robot_constants, - PowerServiceWithDribble& power_service) + const robot_constants::RobotConstants& robot_constants, std::shared_ptr uart) : robot_constants_(robot_constants), reset_gpio_(std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, GpioState::HIGH)), - power_service_(power_service) + uart_(uart) { for (const MotorIndex motor : driveMotors()) { @@ -180,7 +179,7 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, if (motor == MotorIndex::DRIBBLER) { // Here target_velocity is treated as angular velocity with unit RPM - power_service_.dribble(target_velocity); + uart_->sendDribbleTarget(target_velocity); } const auto outgoing_frame = SetTargetSpeedFrame{ diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 270af5983f..b288a1e836 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -5,7 +5,7 @@ #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" -#include "software/embedded/services/power.h" +#include "software/embedded/services/uart_communicator.h" /** * Motor controller for controlling our 6th generation STSPIN motor drivers. @@ -18,7 +18,7 @@ class StSpinMotorController : public MotorController { public: explicit StSpinMotorController(const robot_constants::RobotConstants& robot_constants, - PowerServiceWithDribble& power_service); + std::shared_ptr power_service); void setup() override; @@ -93,7 +93,7 @@ class StSpinMotorController : public MotorController std::unordered_map motor_status_; - PowerServiceWithDribble& power_service_; + std::shared_ptr uart_; /** * Opens a SPI file descriptor for the given motor. 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 1f54b929f5..dd9d26c186 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -8,6 +8,7 @@ #include "software/logger/logger.h" #include "software/logger/network_logger.h" +#include "software/embedded/services/uart_communicator.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; @@ -45,9 +46,9 @@ class StSpinMotorControllerTest ss << motor << " "; } LOG(INFO) << "Enabled motors: " << ss.str(); - + const auto uart =std::make_shared(); const auto motor_controller = std::make_unique( - robot_constants::createRobotConstants()); + robot_constants::createRobotConstants(), uart); motor_controller->setup(); LOG(INFO) << "Motor controller setup complete"; diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index fa9571a16c..787122ba4a 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -30,11 +30,7 @@ cc_library( hdrs = ["power.h"], linkopts = ["-latomic"], # Necessary due to not containing load/store for all sizes deps = [ - "//shared/uart_framing", - "//software/logger", - "//software/uart:boost_uart_communication", - "@boost//:asio", - "@boost//:filesystem", + ":uart_communicator", ], ) @@ -44,10 +40,23 @@ cc_library( hdrs = ["imu.h"], deps = [ "//proto:tbots_cc_proto", - "//shared:constants", + "//shared:constants", "//software/geom:angular_acceleration", "//software/geom:angular_velocity", "//software/logger", "@eigen", ], ) + +cc_library( + name = "uart_communicator", + srcs = ["uart_communicator.cpp"], + hdrs = ["uart_communicator.h"], + deps = [ + "//shared/uart_framing", + "//software/logger", + "//software/uart:boost_uart_communication", + "@boost//:asio", + "@boost//:filesystem", + ] +) \ No newline at end of file diff --git a/src/software/embedded/services/power.cpp b/src/software/embedded/services/power.cpp index 19a7749311..e7361cdc64 100644 --- a/src/software/embedded/services/power.cpp +++ b/src/software/embedded/services/power.cpp @@ -5,139 +5,14 @@ #include "proto/power_frame_msg.pb.h" -PowerService::PowerService() +PowerService::PowerService(std::shared_ptr uart): uart_(uart) { - nanopb_command = createNanoPbPowerPulseControl(TbotsProto::PowerControl(), 0.0, 0, 0); - - if (!boost::filesystem::exists(DEVICE_SERIAL_PORT)) - { - throw std::runtime_error("USB not plugged into the Raspberry Pi"); - } - this->uart = std::make_unique(BAUD_RATE, DEVICE_SERIAL_PORT); - this->read_thread = std::thread(std::bind(&PowerService::continuousRead, this)); -} - -PowerService::~PowerService() -{ - is_running = false; - read_thread.join(); -} - -void PowerService::continuousRead() -{ - while (is_running) - { - tick(); - } -} - -void PowerService::tick() -{ - std::vector power_status; - try - { - uart->flushSerialPort(uart->flush_receive); - power_status = uart->serialRead(READ_BUFFER_SIZE); - } - catch (std::exception& e) - { - LOG(FATAL) << "Read thread has crashed" << e.what(); - } - - TbotsProto_PowerFrame status_frame = TbotsProto_PowerFrame_init_default; - if (!unmarshalUartPacket(power_status, status_frame)) - { - LOG(WARNING) << "Unmarshal failed"; - } - else - { - status = status_frame.power_msg.power_status; - } - - auto command = - nanopb_command.load(std::memory_order_relaxed); // get value atomically - auto frame = createUartFrame(command); - auto power_command_buffer = marshallUartPacket(frame); - - try - { - // Write power command - uart->flushSerialPort(uart->flush_send); - if (!uart->serialWrite(power_command_buffer)) - { - LOG(WARNING) << "Writing power command failed."; - } - } - catch (std::exception& e) - { - LOG(FATAL) << "ESP32 has disconnected. Power service has crashed" << e.what(); - } + } TbotsProto::PowerStatus PowerService::poll(const TbotsProto::PowerControl& command, double kick_coeff, int kick_constant, int chip_constant) { - // Store msg for later transmission - nanopb_command = - createNanoPbPowerPulseControl(command, kick_coeff, kick_constant, chip_constant); - return *createTbotsPowerStatus(status); -} - -void PowerServiceWithDribble::dribble(int rpm) -{ - dribble_command = createNanoPbDribblerControl(rpm); - _new_dribble_command = true; -} - -void PowerServiceWithDribble::tick() -{ - std::vector power_status; - try - { - uart->flushSerialPort(uart->flush_receive); - power_status = uart->serialRead(READ_BUFFER_SIZE); - } - catch (std::exception& e) - { - LOG(FATAL) << "Read thread has crashed" << e.what(); - } - - TbotsProto_PowerFrame status_frame = TbotsProto_PowerFrame_init_default; - if (!unmarshalUartPacket(power_status, status_frame)) - { - LOG(WARNING) << "Unmarshal failed"; - } - else - { - status = status_frame.power_msg.power_status; - } - std::vector power_command_buffer; - if (_new_dribble_command) - { - auto command = - dribble_command.load(std::memory_order_relaxed); // get value atomically - _new_dribble_command = false; - power_command_buffer = marshallUartPacket(createUartFrame(command)); - } - else - { - auto command = - nanopb_command.load(std::memory_order_relaxed); // get value atomically - power_command_buffer = marshallUartPacket(createUartFrame(command)); - } - - try - { - // Write power command - uart->flushSerialPort(uart->flush_send); - if (!uart->serialWrite(power_command_buffer)) - { - LOG(WARNING) << "Writing power command failed."; - } - } - catch (std::exception& e) - { - LOG(FATAL) << "ESP32 has disconnected. Power service has crashed" << e.what(); - } -} + return uart_->sendChipKickCommand(command, kick_coeff, kick_constant, chip_constant); +} \ No newline at end of file diff --git a/src/software/embedded/services/power.h b/src/software/embedded/services/power.h index b3cb5f730d..d19d3b60e5 100644 --- a/src/software/embedded/services/power.h +++ b/src/software/embedded/services/power.h @@ -4,9 +4,7 @@ #include #include "proto/power_frame_msg.pb.h" -#include "shared/uart_framing/uart_framing.hpp" -#include "software/logger/logger.h" -#include "software/uart/boost_uart_communication.h" +#include "software/embedded/services/uart_communicator.h" extern "C" { @@ -18,10 +16,9 @@ class PowerService public: /** * Service that interacts with the power board. - * Opens all the required ports and maintains them until destroyed. + * Connects to the UartCommunicator */ - PowerService(); - ~PowerService(); + PowerService(std::shared_ptr uart); /** * When the power service is polled it sends the given power control msg and @@ -33,54 +30,6 @@ class PowerService TbotsProto::PowerStatus poll(const TbotsProto::PowerControl& control, double kick_coeff, int kick_constant, int chip_constant); - /** - * Handler method called every time the timer expires a new read is requested - */ - virtual void tick(); - - /** - * Set dribbler RPM - */ - void dribble(int rpm); - - private: - /** - * Initiates timer for serial reading - */ - void continuousRead(); - - std::thread read_thread; - std::atomic status; - std::atomic nanopb_command; - std::unique_ptr uart; - - // Constants - const size_t READ_BUFFER_SIZE = - getMarshalledSize(TbotsProto_PowerStatus TbotsProto_PowerStatus_init_default); - const std::string DEVICE_SERIAL_PORT = "/dev/ttyUSB0"; - static constexpr unsigned int BAUD_RATE = 460800; - - // Required flag to exit power service cleanly - bool is_running = true; -}; - -/** - * A separate power service that also controls the dribbler. - */ -class PowerServiceWithDribble : public PowerService -{ - public: - /** - * Handler method called every time the timer expires a new read is requested - */ - void tick() override; - - /** - * Set dribbler RPM - */ - void dribble(int rpm); - private: - bool _new_dribble_command = false; - std::atomic dribble_command; + std::shared_ptr uart_; }; diff --git a/src/software/embedded/services/uart_communicator.cpp b/src/software/embedded/services/uart_communicator.cpp new file mode 100644 index 0000000000..532d16ed62 --- /dev/null +++ b/src/software/embedded/services/uart_communicator.cpp @@ -0,0 +1,102 @@ +#include "software/embedded/services/uart_communicator.h" + +#include +#include + +#include "proto/power_frame_msg.pb.h" + +UartCommunicator::UartCommunicator() +{ + nanopb_command = createNanoPbPowerPulseControl(TbotsProto::PowerControl(), 0.0, 0, 0); + + if (!boost::filesystem::exists(DEVICE_SERIAL_PORT)) + { + throw std::runtime_error("USB not plugged into the Raspberry Pi"); + } + this->uart = std::make_unique(BAUD_RATE, DEVICE_SERIAL_PORT); + this->read_thread = std::thread(std::bind(&UartCommunicator::continuousRead, this)); +} + +UartCommunicator::~UartCommunicator() +{ + is_running = false; + read_thread.join(); +} + + +void UartCommunicator::sendDribbleTarget(int rpm) +{ + dribble_command = createNanoPbDribblerControl(rpm); + _new_dribble_command = true; +} + +TbotsProto::PowerStatus UartCommunicator::sendChipKickCommand(const TbotsProto::PowerControl& command, + double kick_coeff, int kick_constant, + int chip_constant) +{ + // Store msg for later transmission + nanopb_command = + createNanoPbPowerPulseControl(command, kick_coeff, kick_constant, chip_constant); + return *createTbotsPowerStatus(status); +} + +void UartCommunicator::tick() +{ + std::vector power_status; + try + { + uart->flushSerialPort(uart->flush_receive); + power_status = uart->serialRead(READ_BUFFER_SIZE); + } + catch (std::exception& e) + { + LOG(FATAL) << "Read thread has crashed" << e.what(); + } + + TbotsProto_PowerFrame status_frame = TbotsProto_PowerFrame_init_default; + if (!unmarshalUartPacket(power_status, status_frame)) + { + LOG(WARNING) << "Unmarshal failed"; + } + else + { + status = status_frame.power_msg.power_status; + } + + std::vector power_command_buffer; + if (_new_dribble_command) + { + auto command = + dribble_command.load(std::memory_order_relaxed); // get value atomically + _new_dribble_command = false; + power_command_buffer = marshallUartPacket(createUartFrame(command)); + } + else + { + auto command = + nanopb_command.load(std::memory_order_relaxed); // get value atomically + power_command_buffer = marshallUartPacket(createUartFrame(command)); + } + + try + { + // Write power command + uart->flushSerialPort(uart->flush_send); + if (!uart->serialWrite(power_command_buffer)) + { + LOG(WARNING) << "Writing power command failed."; + } + } + catch (std::exception& e) + { + LOG(FATAL) << "ESP32 has disconnected. Power service has crashed" << e.what(); + } +} + +void UartCommunicator::continuousRead() +{ + while (is_running) + { + tick(); + } +} \ No newline at end of file diff --git a/src/software/embedded/services/uart_communicator.h b/src/software/embedded/services/uart_communicator.h new file mode 100644 index 0000000000..70c27fa1e8 --- /dev/null +++ b/src/software/embedded/services/uart_communicator.h @@ -0,0 +1,71 @@ +#pragma once + +#include +#include + +#include "proto/power_frame_msg.pb.h" +#include "shared/uart_framing/uart_framing.hpp" +#include "software/logger/logger.h" +#include "software/uart/boost_uart_communication.h" + +extern "C" +{ +#include "proto/power_frame_msg.pb.h" +} + +/** + * Communicates to the powerboard over uart. + */ +class UartCommunicator +{ + public: + /** + * Service that interacts with the power board. + * Opens all the required ports and maintains them until destroyed. + */ + UartCommunicator(); + ~UartCommunicator(); + + /** + * When the power service is polled it sends the given power control msg and + * returns the latest power status + * + * @param control The power control msg to send + * @return the latest power status + */ + TbotsProto::PowerStatus sendChipKickCommand(const TbotsProto::PowerControl& control, + double kick_coeff, int kick_constant, int chip_constant); + + /** + * Set dribbler RPM + */ + void sendDribbleTarget(int rpm); + + /** + * Handler method called every time the timer expires a new read is requested + */ + virtual void tick(); + + protected: + /** + * Initiates timer for serial reading + */ + void continuousRead(); + + std::thread read_thread; + std::atomic status; + std::atomic nanopb_command; + std::unique_ptr uart; + + // Constants + const size_t READ_BUFFER_SIZE = + getMarshalledSize(TbotsProto_PowerStatus TbotsProto_PowerStatus_init_default); + const std::string DEVICE_SERIAL_PORT = "/dev/ttyUSB0"; + static constexpr unsigned int BAUD_RATE = 460800; + + // Required flag to exit power service cleanly + bool is_running = true; + + bool _new_dribble_command = false; + std::atomic dribble_command; +}; \ No newline at end of file diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 331107c0ec..1638ba5682 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -9,9 +9,6 @@ #include "proto/tbots_software_msgs.pb.h" #include "shared/constants.h" #include "software/constants.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/embedded/primitive_executor.h" #include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" @@ -20,6 +17,10 @@ #include "software/networking/tbots_network_exception.h" #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.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/embedded/services/uart_communicator.h" /** * https://web.archive.org/web/20210308013218/https://rt.wiki.kernel.org/index.php/Squarewave-example @@ -120,24 +121,18 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, LOG(INFO) << "THUNDERLOOP: Network Service initialized! Next initializing Power Service"; - + std::shared_ptr uart = std::make_shared(); + power_service_ = std::make_unique(uart); + LOG(INFO) + << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) { - power_service_ = std::make_unique(); - LOG(INFO) - << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; - motor_service_ = std::make_unique( - robot_constants, std::make_unique()); + motor_service_ = std::make_unique(robot_constants,std::make_unique()); } else { - power_service_ = std::make_unique(); - LOG(INFO) - << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; - motor_service_ = std::make_unique( - robot_constants, - std::make_unique(robot_constants, power_service_)); + motor_service_ = std::make_unique(robot_constants, std::make_unique(robot_constants, uart)); } g_motor_service = motor_service_.get(); motor_service_->setup(); From 2e24ca8c3a310a686de1b513b296535993f83cfc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Fri, 5 Jun 2026 06:47:57 +0000 Subject: [PATCH 32/34] [pre-commit.ci lite] apply automatic fixes --- .../embedded/ansible/requirements_lock.txt | 333 ++++++++++++++++++ src/software/embedded/motor_controller/BUILD | 2 +- .../stspin_motor_controller.cpp | 3 +- .../stspin_motor_controller.h | 2 +- .../stspin_motor_controller_test.cpp | 4 +- src/software/embedded/services/BUILD | 6 +- src/software/embedded/services/power.cpp | 7 +- .../embedded/services/uart_communicator.cpp | 8 +- .../embedded/services/uart_communicator.h | 7 +- src/software/embedded/thunderloop.cpp | 31 +- 10 files changed, 370 insertions(+), 33 deletions(-) diff --git a/src/software/embedded/ansible/requirements_lock.txt b/src/software/embedded/ansible/requirements_lock.txt index e69de29bb2..2f2f0a02fd 100644 --- a/src/software/embedded/ansible/requirements_lock.txt +++ b/src/software/embedded/ansible/requirements_lock.txt @@ -0,0 +1,333 @@ +# +# This file is autogenerated by pip-compile with Python 3.12 +# by the following command: +# +# bazel run //software/embedded/ansible:requirements.update +# +ansible==11.9.0 \ + --hash=sha256:528ca5a408f11cf1fea00daea7570e68d40e167be38b90c119a7cb45729e4921 \ + --hash=sha256:79b087ef38105b93e0e092e7013a0f840e154a6a8ce9b5fddd1b47593adc542a + # via -r software/embedded/ansible/requirements.in +ansible-core==2.18.17 \ + --hash=sha256:556815258f84a57349b63474479506b2a8431a85661fd3f53a5c11894b7a3f25 \ + --hash=sha256:ae746d8b76c45cc7ee4123fba1f9ea1e3df2ed36a5d2b7ace7e88205bacc73f9 + # via ansible +cffi==2.0.0 \ + --hash=sha256:00bdf7acc5f795150faa6957054fbbca2439db2f775ce831222b66f192f03beb \ + --hash=sha256:07b271772c100085dd28b74fa0cd81c8fb1a3ba18b21e03d7c27f3436a10606b \ + --hash=sha256:087067fa8953339c723661eda6b54bc98c5625757ea62e95eb4898ad5e776e9f \ + --hash=sha256:0a1527a803f0a659de1af2e1fd700213caba79377e27e4693648c2923da066f9 \ + --hash=sha256:0cf2d91ecc3fcc0625c2c530fe004f82c110405f101548512cce44322fa8ac44 \ + --hash=sha256:0f6084a0ea23d05d20c3edcda20c3d006f9b6f3fefeac38f59262e10cef47ee2 \ + --hash=sha256:12873ca6cb9b0f0d3a0da705d6086fe911591737a59f28b7936bdfed27c0d47c \ + --hash=sha256:19f705ada2530c1167abacb171925dd886168931e0a7b78f5bffcae5c6b5be75 \ + --hash=sha256:1cd13c99ce269b3ed80b417dcd591415d3372bcac067009b6e0f59c7d4015e65 \ + --hash=sha256:1e3a615586f05fc4065a8b22b8152f0c1b00cdbc60596d187c2a74f9e3036e4e \ + --hash=sha256:1f72fb8906754ac8a2cc3f9f5aaa298070652a0ffae577e0ea9bd480dc3c931a \ + --hash=sha256:1fc9ea04857caf665289b7a75923f2c6ed559b8298a1b8c49e59f7dd95c8481e \ + --hash=sha256:203a48d1fb583fc7d78a4c6655692963b860a417c0528492a6bc21f1aaefab25 \ + --hash=sha256:2081580ebb843f759b9f617314a24ed5738c51d2aee65d31e02f6f7a2b97707a \ + --hash=sha256:21d1152871b019407d8ac3985f6775c079416c282e431a4da6afe7aefd2bccbe \ + --hash=sha256:24b6f81f1983e6df8db3adc38562c83f7d4a0c36162885ec7f7b77c7dcbec97b \ + --hash=sha256:256f80b80ca3853f90c21b23ee78cd008713787b1b1e93eae9f3d6a7134abd91 \ + --hash=sha256:28a3a209b96630bca57cce802da70c266eb08c6e97e5afd61a75611ee6c64592 \ + --hash=sha256:2c8f814d84194c9ea681642fd164267891702542f028a15fc97d4674b6206187 \ + --hash=sha256:2de9a304e27f7596cd03d16f1b7c72219bd944e99cc52b84d0145aefb07cbd3c \ + --hash=sha256:38100abb9d1b1435bc4cc340bb4489635dc2f0da7456590877030c9b3d40b0c1 \ + --hash=sha256:3925dd22fa2b7699ed2617149842d2e6adde22b262fcbfada50e3d195e4b3a94 \ + --hash=sha256:3e17ed538242334bf70832644a32a7aae3d83b57567f9fd60a26257e992b79ba \ + --hash=sha256:3e837e369566884707ddaf85fc1744b47575005c0a229de3327f8f9a20f4efeb \ + --hash=sha256:3f4d46d8b35698056ec29bca21546e1551a205058ae1a181d871e278b0b28165 \ + --hash=sha256:44d1b5909021139fe36001ae048dbdde8214afa20200eda0f64c068cac5d5529 \ + --hash=sha256:45d5e886156860dc35862657e1494b9bae8dfa63bf56796f2fb56e1679fc0bca \ + --hash=sha256:4647afc2f90d1ddd33441e5b0e85b16b12ddec4fca55f0d9671fef036ecca27c \ + --hash=sha256:4671d9dd5ec934cb9a73e7ee9676f9362aba54f7f34910956b84d727b0d73fb6 \ + --hash=sha256:53f77cbe57044e88bbd5ed26ac1d0514d2acf0591dd6bb02a3ae37f76811b80c \ + --hash=sha256:5eda85d6d1879e692d546a078b44251cdd08dd1cfb98dfb77b670c97cee49ea0 \ + --hash=sha256:5fed36fccc0612a53f1d4d9a816b50a36702c28a2aa880cb8a122b3466638743 \ + --hash=sha256:61d028e90346df14fedc3d1e5441df818d095f3b87d286825dfcbd6459b7ef63 \ + --hash=sha256:66f011380d0e49ed280c789fbd08ff0d40968ee7b665575489afa95c98196ab5 \ + --hash=sha256:6824f87845e3396029f3820c206e459ccc91760e8fa24422f8b0c3d1731cbec5 \ + --hash=sha256:6c6c373cfc5c83a975506110d17457138c8c63016b563cc9ed6e056a82f13ce4 \ + --hash=sha256:6d02d6655b0e54f54c4ef0b94eb6be0607b70853c45ce98bd278dc7de718be5d \ + --hash=sha256:6d50360be4546678fc1b79ffe7a66265e28667840010348dd69a314145807a1b \ + --hash=sha256:730cacb21e1bdff3ce90babf007d0a0917cc3e6492f336c2f0134101e0944f93 \ + --hash=sha256:737fe7d37e1a1bffe70bd5754ea763a62a066dc5913ca57e957824b72a85e205 \ + --hash=sha256:74a03b9698e198d47562765773b4a8309919089150a0bb17d829ad7b44b60d27 \ + --hash=sha256:7553fb2090d71822f02c629afe6042c299edf91ba1bf94951165613553984512 \ + --hash=sha256:7a66c7204d8869299919db4d5069a82f1561581af12b11b3c9f48c584eb8743d \ + --hash=sha256:7cc09976e8b56f8cebd752f7113ad07752461f48a58cbba644139015ac24954c \ + --hash=sha256:81afed14892743bbe14dacb9e36d9e0e504cd204e0b165062c488942b9718037 \ + --hash=sha256:8941aaadaf67246224cee8c3803777eed332a19d909b47e29c9842ef1e79ac26 \ + --hash=sha256:89472c9762729b5ae1ad974b777416bfda4ac5642423fa93bd57a09204712322 \ + --hash=sha256:8ea985900c5c95ce9db1745f7933eeef5d314f0565b27625d9a10ec9881e1bfb \ + --hash=sha256:8eca2a813c1cb7ad4fb74d368c2ffbbb4789d377ee5bb8df98373c2cc0dee76c \ + --hash=sha256:92b68146a71df78564e4ef48af17551a5ddd142e5190cdf2c5624d0c3ff5b2e8 \ + --hash=sha256:9332088d75dc3241c702d852d4671613136d90fa6881da7d770a483fd05248b4 \ + --hash=sha256:94698a9c5f91f9d138526b48fe26a199609544591f859c870d477351dc7b2414 \ + --hash=sha256:9a67fc9e8eb39039280526379fb3a70023d77caec1852002b4da7e8b270c4dd9 \ + --hash=sha256:9de40a7b0323d889cf8d23d1ef214f565ab154443c42737dfe52ff82cf857664 \ + --hash=sha256:a05d0c237b3349096d3981b727493e22147f934b20f6f125a3eba8f994bec4a9 \ + --hash=sha256:afb8db5439b81cf9c9d0c80404b60c3cc9c3add93e114dcae767f1477cb53775 \ + --hash=sha256:b18a3ed7d5b3bd8d9ef7a8cb226502c6bf8308df1525e1cc676c3680e7176739 \ + --hash=sha256:b1e74d11748e7e98e2f426ab176d4ed720a64412b6a15054378afdb71e0f37dc \ + --hash=sha256:b21e08af67b8a103c71a250401c78d5e0893beff75e28c53c98f4de42f774062 \ + --hash=sha256:b4c854ef3adc177950a8dfc81a86f5115d2abd545751a304c5bcf2c2c7283cfe \ + --hash=sha256:b882b3df248017dba09d6b16defe9b5c407fe32fc7c65a9c69798e6175601be9 \ + --hash=sha256:baf5215e0ab74c16e2dd324e8ec067ef59e41125d3eade2b863d294fd5035c92 \ + --hash=sha256:c649e3a33450ec82378822b3dad03cc228b8f5963c0c12fc3b1e0ab940f768a5 \ + --hash=sha256:c654de545946e0db659b3400168c9ad31b5d29593291482c43e3564effbcee13 \ + --hash=sha256:c6638687455baf640e37344fe26d37c404db8b80d037c3d29f58fe8d1c3b194d \ + --hash=sha256:c8d3b5532fc71b7a77c09192b4a5a200ea992702734a2e9279a37f2478236f26 \ + --hash=sha256:cb527a79772e5ef98fb1d700678fe031e353e765d1ca2d409c92263c6d43e09f \ + --hash=sha256:cf364028c016c03078a23b503f02058f1814320a56ad535686f90565636a9495 \ + --hash=sha256:d48a880098c96020b02d5a1f7d9251308510ce8858940e6fa99ece33f610838b \ + --hash=sha256:d68b6cef7827e8641e8ef16f4494edda8b36104d79773a334beaa1e3521430f6 \ + --hash=sha256:d9b29c1f0ae438d5ee9acb31cadee00a58c46cc9c0b2f9038c6b0b3470877a8c \ + --hash=sha256:d9b97165e8aed9272a6bb17c01e3cc5871a594a446ebedc996e2397a1c1ea8ef \ + --hash=sha256:da68248800ad6320861f129cd9c1bf96ca849a2771a59e0344e88681905916f5 \ + --hash=sha256:da902562c3e9c550df360bfa53c035b2f241fed6d9aef119048073680ace4a18 \ + --hash=sha256:dbd5c7a25a7cb98f5ca55d258b103a2054f859a46ae11aaf23134f9cc0d356ad \ + --hash=sha256:dd4f05f54a52fb558f1ba9f528228066954fee3ebe629fc1660d874d040ae5a3 \ + --hash=sha256:de8dad4425a6ca6e4e5e297b27b5c824ecc7581910bf9aee86cb6835e6812aa7 \ + --hash=sha256:e11e82b744887154b182fd3e7e8512418446501191994dbf9c9fc1f32cc8efd5 \ + --hash=sha256:e6e73b9e02893c764e7e8d5bb5ce277f1a009cd5243f8228f75f842bf937c534 \ + --hash=sha256:f73b96c41e3b2adedc34a7356e64c8eb96e03a3782b535e043a986276ce12a49 \ + --hash=sha256:f93fd8e5c8c0a4aa1f424d6173f14a892044054871c771f8566e4008eaa359d2 \ + --hash=sha256:fc33c5141b55ed366cfaad382df24fe7dcbc686de5be719b207bb248e3053dc5 \ + --hash=sha256:fc7de24befaeae77ba923797c7c87834c73648a05a4bde34b3b7e5588973a453 \ + --hash=sha256:fe562eb1a64e67dd297ccc4f5addea2501664954f2692b69a76449ec7913ecbf + # via cryptography +cryptography==48.0.0 \ + --hash=sha256:0890f502ddf7d9c6426129c3f49f5c0a39278ed7cd6322c8755ffca6ee675a13 \ + --hash=sha256:0c558d2cdffd8f4bbb30fc7134c74d2ca9a476f830bb053074498fbc86f41ed6 \ + --hash=sha256:16cd65b9330583e4619939b3a3843eec1e6e789744bb01e7c7e2e62e33c239c8 \ + --hash=sha256:18349bbc56f4743c8b12dc32e2bccb2cf83ee8b69a3bba74ef8ae857e26b3d25 \ + --hash=sha256:1e2d54c8be6152856a36f0882ab231e70f8ec7f14e93cf87db8a2ed056bf160c \ + --hash=sha256:22a5cb272895dce158b2cacdfdc3debd299019659f42947dbdac6f32d68fe832 \ + --hash=sha256:27241b1dc9962e056062a8eef1991d02c3a24569c95975bd2322a8a52c6e5e12 \ + --hash=sha256:2b4d59804e8408e2fea7d1fbaf218e5ec984325221db76e6a241a9abd6cdd95c \ + --hash=sha256:2eb992bbd4661238c5a397594c83f5b4dc2bc5b848c365c8f991b6780efcc5c7 \ + --hash=sha256:369a6348999f94bbd53435c894377b20ab95f25a9065c283570e70150d8abc3c \ + --hash=sha256:3cb07a3ed6431663cd321ea8a000a1314c74211f823e4177fefa2255e057d1ec \ + --hash=sha256:40ba1f85eaa6959837b1d51c9767e230e14612eea4ef110ee8854ada22da1bf5 \ + --hash=sha256:4defde8685ae324a9eb9d818717e93b4638ef67070ac9bc15b8ca85f63048355 \ + --hash=sha256:55b7718303bf06a5753dcdccf2f3945cf18ad7bffde41b61226e4db31ab89a9c \ + --hash=sha256:561215ea3879cb1cbbf272867e2efda62476f240fb58c64de6b393ae19246741 \ + --hash=sha256:58d00498e8933e4a194f3076aee1b4a97dfec1a6da444535755822fe5d8b0b86 \ + --hash=sha256:59baa2cb386c4f0b9905bd6eb4c2a79a69a128408fd31d32ca4d7102d4156321 \ + --hash=sha256:5a5ed8fde7a1d09376ca0b40e68cd59c69fe23b1f9768bd5824f54681626032a \ + --hash=sha256:5b012212e08b8dd5edc78ef54da83dd9892fd9105323b3993eff6bea65dc21d7 \ + --hash=sha256:5c3932f4436d1cccb036cb0eaef46e6e2db91035166f1ad6505c3c9d5a635920 \ + --hash=sha256:614d0949f4790582d2cc25553abd09dd723025f0c0e7c67376a1d77196743d6e \ + --hash=sha256:76341972e1eff8b4bea859f09c0d3e64b96ce931b084f9b9b7db8ef364c30eff \ + --hash=sha256:77a2ccbbe917f6710e05ba9adaa25fb5075620bf3ea6fb751997875aff4ae4bd \ + --hash=sha256:7995ef305d7165c3f11ae07f2517e5a4f1d5c18da1376a0a9ed496336b69e5f3 \ + --hash=sha256:7ce4bfae76319a532a2dc68f82cc32f5676ee792a983187dac07183690e5c66f \ + --hash=sha256:7e8eac43dfca5c4cccc6dad9a80504436fca53bb9bc3100a2386d730fbe6b602 \ + --hash=sha256:84cf79f0dc8b36ac5da873481716e87aef31fcfa0444f9e1d8b4b2cece142855 \ + --hash=sha256:8c7378637d7d88016fa6791c159f698b3d3eed28ebf844ac36b9dc04a14dae18 \ + --hash=sha256:8cd666227ef7af430aa5914a9910e0ddd703e75f039cef0825cd0da71b6b711a \ + --hash=sha256:906cbf0670286c6e0044156bc7d4af9cbb0ef6db9f73e52c3ec56ba6bdde5336 \ + --hash=sha256:9071196d81abc88b3516ac8cdfad32e2b66dd4a5393a8e68a961e9161ddc6239 \ + --hash=sha256:9249e3cd978541d665967ac2cb2787fd6a62bddf1e75b3e347a594d7dacf4f74 \ + --hash=sha256:984a20b0f62a26f48a3396c72e4bc34c66e356d356bf370053066b3b6d54634a \ + --hash=sha256:9be5aafa5736574f8f15f262adc81b2a9869e2cfe9014d52a44633905b40d52c \ + --hash=sha256:9c459db21422be75e2809370b829a87eb37f74cd785fc4aa9ea1e5f43b47cda4 \ + --hash=sha256:9ccdac7d40688ecb5a3b4a604b8a88c8002e3442d6c60aead1db2a89a041560c \ + --hash=sha256:a0e692c683f4df67815a2d258b324e66f4738bd7a96a218c826dce4f4bd05d8f \ + --hash=sha256:a5da777e32ffed6f85a7b2b3f7c5cbc88c146bfcd0a1d7baf5fcc6c52ee35dd4 \ + --hash=sha256:a64697c641c7b1b2178e573cbc31c7c6684cd56883a478d75143dbb7118036db \ + --hash=sha256:ad64688338ed4bc1a6618076ba75fd7194a5f1797ac60b47afe926285adb3166 \ + --hash=sha256:bd72e68b06bb1e96913f97dd4901119bc17f39d4586a5adf2d3e47bc2b9d58b5 \ + --hash=sha256:c17dfe85494deaeddc5ce251aebd1d60bbe6afc8b62071bb0b469431a000124f \ + --hash=sha256:c18684a7f0cc9a3cb60328f496b8e3372def7c5d2df39ac267878b05565aaaae \ + --hash=sha256:cc90c0b39b2e3c65ef52c804b72e3c58f8a04ab2a1871272798e5f9572c17d20 \ + --hash=sha256:db63bf618e5dea46c07de12e900fe1cdd2541e6dc9dbae772a70b7d4d4765f6a \ + --hash=sha256:ea8990436d914540a40ab24b6a77c0969695ed52f4a4874c5137ccf7045a7057 \ + --hash=sha256:ecde28a596bead48b0cfd2a1b4416c3d43074c2d785e3a398d7ec1fc4d0f7fbb \ + --hash=sha256:f5333311663ea94f75dd408665686aaf426563556bb5283554a3539177e03b8c \ + --hash=sha256:fdfef35d751d510fcef5252703621574364fec16418c4a1e5e1055248401054b + # via ansible-core +jinja2==3.1.6 \ + --hash=sha256:0137fb05990d35f1275a587e9aee6d56da821fc83491a0fb838183be43f66d6d \ + --hash=sha256:85ece4451f492d0c13c5dd7c13a64681a86afae63a5f347908daf103ce6d2f67 + # via ansible-core +markupsafe==3.0.3 \ + --hash=sha256:0303439a41979d9e74d18ff5e2dd8c43ed6c6001fd40e5bf2e43f7bd9bbc523f \ + --hash=sha256:068f375c472b3e7acbe2d5318dea141359e6900156b5b2ba06a30b169086b91a \ + --hash=sha256:0bf2a864d67e76e5c9a34dc26ec616a66b9888e25e7b9460e1c76d3293bd9dbf \ + --hash=sha256:0db14f5dafddbb6d9208827849fad01f1a2609380add406671a26386cdf15a19 \ + --hash=sha256:0eb9ff8191e8498cca014656ae6b8d61f39da5f95b488805da4bb029cccbfbaf \ + --hash=sha256:0f4b68347f8c5eab4a13419215bdfd7f8c9b19f2b25520968adfad23eb0ce60c \ + --hash=sha256:1085e7fbddd3be5f89cc898938f42c0b3c711fdcb37d75221de2666af647c175 \ + --hash=sha256:116bb52f642a37c115f517494ea5feb03889e04df47eeff5b130b1808ce7c219 \ + --hash=sha256:12c63dfb4a98206f045aa9563db46507995f7ef6d83b2f68eda65c307c6829eb \ + --hash=sha256:133a43e73a802c5562be9bbcd03d090aa5a1fe899db609c29e8c8d815c5f6de6 \ + --hash=sha256:1353ef0c1b138e1907ae78e2f6c63ff67501122006b0f9abad68fda5f4ffc6ab \ + --hash=sha256:15d939a21d546304880945ca1ecb8a039db6b4dc49b2c5a400387cdae6a62e26 \ + --hash=sha256:177b5253b2834fe3678cb4a5f0059808258584c559193998be2601324fdeafb1 \ + --hash=sha256:1872df69a4de6aead3491198eaf13810b565bdbeec3ae2dc8780f14458ec73ce \ + --hash=sha256:1b4b79e8ebf6b55351f0d91fe80f893b4743f104bff22e90697db1590e47a218 \ + --hash=sha256:1b52b4fb9df4eb9ae465f8d0c228a00624de2334f216f178a995ccdcf82c4634 \ + --hash=sha256:1ba88449deb3de88bd40044603fafffb7bc2b055d626a330323a9ed736661695 \ + --hash=sha256:1cc7ea17a6824959616c525620e387f6dd30fec8cb44f649e31712db02123dad \ + --hash=sha256:218551f6df4868a8d527e3062d0fb968682fe92054e89978594c28e642c43a73 \ + --hash=sha256:26a5784ded40c9e318cfc2bdb30fe164bdb8665ded9cd64d500a34fb42067b1c \ + --hash=sha256:2713baf880df847f2bece4230d4d094280f4e67b1e813eec43b4c0e144a34ffe \ + --hash=sha256:2a15a08b17dd94c53a1da0438822d70ebcd13f8c3a95abe3a9ef9f11a94830aa \ + --hash=sha256:2f981d352f04553a7171b8e44369f2af4055f888dfb147d55e42d29e29e74559 \ + --hash=sha256:32001d6a8fc98c8cb5c947787c5d08b0a50663d139f1305bac5885d98d9b40fa \ + --hash=sha256:3524b778fe5cfb3452a09d31e7b5adefeea8c5be1d43c4f810ba09f2ceb29d37 \ + --hash=sha256:3537e01efc9d4dccdf77221fb1cb3b8e1a38d5428920e0657ce299b20324d758 \ + --hash=sha256:35add3b638a5d900e807944a078b51922212fb3dedb01633a8defc4b01a3c85f \ + --hash=sha256:38664109c14ffc9e7437e86b4dceb442b0096dfe3541d7864d9cbe1da4cf36c8 \ + --hash=sha256:3a7e8ae81ae39e62a41ec302f972ba6ae23a5c5396c8e60113e9066ef893da0d \ + --hash=sha256:3b562dd9e9ea93f13d53989d23a7e775fdfd1066c33494ff43f5418bc8c58a5c \ + --hash=sha256:457a69a9577064c05a97c41f4e65148652db078a3a509039e64d3467b9e7ef97 \ + --hash=sha256:4bd4cd07944443f5a265608cc6aab442e4f74dff8088b0dfc8238647b8f6ae9a \ + --hash=sha256:4e885a3d1efa2eadc93c894a21770e4bc67899e3543680313b09f139e149ab19 \ + --hash=sha256:4faffd047e07c38848ce017e8725090413cd80cbc23d86e55c587bf979e579c9 \ + --hash=sha256:509fa21c6deb7a7a273d629cf5ec029bc209d1a51178615ddf718f5918992ab9 \ + --hash=sha256:5678211cb9333a6468fb8d8be0305520aa073f50d17f089b5b4b477ea6e67fdc \ + --hash=sha256:591ae9f2a647529ca990bc681daebdd52c8791ff06c2bfa05b65163e28102ef2 \ + --hash=sha256:5a7d5dc5140555cf21a6fefbdbf8723f06fcd2f63ef108f2854de715e4422cb4 \ + --hash=sha256:69c0b73548bc525c8cb9a251cddf1931d1db4d2258e9599c28c07ef3580ef354 \ + --hash=sha256:6b5420a1d9450023228968e7e6a9ce57f65d148ab56d2313fcd589eee96a7a50 \ + --hash=sha256:722695808f4b6457b320fdc131280796bdceb04ab50fe1795cd540799ebe1698 \ + --hash=sha256:729586769a26dbceff69f7a7dbbf59ab6572b99d94576a5592625d5b411576b9 \ + --hash=sha256:77f0643abe7495da77fb436f50f8dab76dbc6e5fd25d39589a0f1fe6548bfa2b \ + --hash=sha256:795e7751525cae078558e679d646ae45574b47ed6e7771863fcc079a6171a0fc \ + --hash=sha256:7be7b61bb172e1ed687f1754f8e7484f1c8019780f6f6b0786e76bb01c2ae115 \ + --hash=sha256:7c3fb7d25180895632e5d3148dbdc29ea38ccb7fd210aa27acbd1201a1902c6e \ + --hash=sha256:7e68f88e5b8799aa49c85cd116c932a1ac15caaa3f5db09087854d218359e485 \ + --hash=sha256:83891d0e9fb81a825d9a6d61e3f07550ca70a076484292a70fde82c4b807286f \ + --hash=sha256:8485f406a96febb5140bfeca44a73e3ce5116b2501ac54fe953e488fb1d03b12 \ + --hash=sha256:8709b08f4a89aa7586de0aadc8da56180242ee0ada3999749b183aa23df95025 \ + --hash=sha256:8f71bc33915be5186016f675cd83a1e08523649b0e33efdb898db577ef5bb009 \ + --hash=sha256:915c04ba3851909ce68ccc2b8e2cd691618c4dc4c4232fb7982bca3f41fd8c3d \ + --hash=sha256:949b8d66bc381ee8b007cd945914c721d9aba8e27f71959d750a46f7c282b20b \ + --hash=sha256:94c6f0bb423f739146aec64595853541634bde58b2135f27f61c1ffd1cd4d16a \ + --hash=sha256:9a1abfdc021a164803f4d485104931fb8f8c1efd55bc6b748d2f5774e78b62c5 \ + --hash=sha256:9b79b7a16f7fedff2495d684f2b59b0457c3b493778c9eed31111be64d58279f \ + --hash=sha256:a320721ab5a1aba0a233739394eb907f8c8da5c98c9181d1161e77a0c8e36f2d \ + --hash=sha256:a4afe79fb3de0b7097d81da19090f4df4f8d3a2b3adaa8764138aac2e44f3af1 \ + --hash=sha256:ad2cf8aa28b8c020ab2fc8287b0f823d0a7d8630784c31e9ee5edea20f406287 \ + --hash=sha256:b8512a91625c9b3da6f127803b166b629725e68af71f8184ae7e7d54686a56d6 \ + --hash=sha256:bc51efed119bc9cfdf792cdeaa4d67e8f6fcccab66ed4bfdd6bde3e59bfcbb2f \ + --hash=sha256:bdc919ead48f234740ad807933cdf545180bfbe9342c2bb451556db2ed958581 \ + --hash=sha256:bdd37121970bfd8be76c5fb069c7751683bdf373db1ed6c010162b2a130248ed \ + --hash=sha256:be8813b57049a7dc738189df53d69395eba14fb99345e0a5994914a3864c8a4b \ + --hash=sha256:c0c0b3ade1c0b13b936d7970b1d37a57acde9199dc2aecc4c336773e1d86049c \ + --hash=sha256:c47a551199eb8eb2121d4f0f15ae0f923d31350ab9280078d1e5f12b249e0026 \ + --hash=sha256:c4ffb7ebf07cfe8931028e3e4c85f0357459a3f9f9490886198848f4fa002ec8 \ + --hash=sha256:ccfcd093f13f0f0b7fdd0f198b90053bf7b2f02a3927a30e63f3ccc9df56b676 \ + --hash=sha256:d2ee202e79d8ed691ceebae8e0486bd9a2cd4794cec4824e1c99b6f5009502f6 \ + --hash=sha256:d53197da72cc091b024dd97249dfc7794d6a56530370992a5e1a08983ad9230e \ + --hash=sha256:d6dd0be5b5b189d31db7cda48b91d7e0a9795f31430b7f271219ab30f1d3ac9d \ + --hash=sha256:d88b440e37a16e651bda4c7c2b930eb586fd15ca7406cb39e211fcff3bf3017d \ + --hash=sha256:de8a88e63464af587c950061a5e6a67d3632e36df62b986892331d4620a35c01 \ + --hash=sha256:df2449253ef108a379b8b5d6b43f4b1a8e81a061d6537becd5582fba5f9196d7 \ + --hash=sha256:e1c1493fb6e50ab01d20a22826e57520f1284df32f2d8601fdd90b6304601419 \ + --hash=sha256:e1cf1972137e83c5d4c136c43ced9ac51d0e124706ee1c8aa8532c1287fa8795 \ + --hash=sha256:e2103a929dfa2fcaf9bb4e7c091983a49c9ac3b19c9061b6d5427dd7d14d81a1 \ + --hash=sha256:e56b7d45a839a697b5eb268c82a71bd8c7f6c94d6fd50c3d577fa39a9f1409f5 \ + --hash=sha256:e8afc3f2ccfa24215f8cb28dcf43f0113ac3c37c2f0f0806d8c70e4228c5cf4d \ + --hash=sha256:e8fc20152abba6b83724d7ff268c249fa196d8259ff481f3b1476383f8f24e42 \ + --hash=sha256:eaa9599de571d72e2daf60164784109f19978b327a3910d3e9de8c97b5b70cfe \ + --hash=sha256:ec15a59cf5af7be74194f7ab02d0f59a62bdcf1a537677ce67a2537c9b87fcda \ + --hash=sha256:f190daf01f13c72eac4efd5c430a8de82489d9cff23c364c3ea822545032993e \ + --hash=sha256:f34c41761022dd093b4b6896d4810782ffbabe30f2d443ff5f083e0cbbb8c737 \ + --hash=sha256:f3e98bb3798ead92273dc0e5fd0f31ade220f59a266ffd8a4f6065e0a3ce0523 \ + --hash=sha256:f42d0984e947b8adf7dd6dde396e720934d12c506ce84eea8476409563607591 \ + --hash=sha256:f71a396b3bf33ecaa1626c255855702aca4d3d9fea5e051b41ac59a9c1c41edc \ + --hash=sha256:f9e130248f4462aaa8e2552d547f36ddadbeaa573879158d721bbd33dfe4743a \ + --hash=sha256:fed51ac40f757d41b7c48425901843666a6677e3e8eb0abcff09e4ba6e664f50 + # via jinja2 +packaging==26.2 \ + --hash=sha256:5fc45236b9446107ff2415ce77c807cee2862cb6fac22b8a73826d0693b0980e \ + --hash=sha256:ff452ff5a3e828ce110190feff1178bb1f2ea2281fa2075aadb987c2fb221661 + # via ansible-core +pycparser==3.0 \ + --hash=sha256:600f49d217304a5902ac3c37e1281c9fe94e4d0489de643a9504c5cdfdfc6b29 \ + --hash=sha256:b727414169a36b7d524c1c3e31839a521725078d7b2ff038656844266160a992 + # via cffi +pyyaml==6.0.3 \ + --hash=sha256:00c4bdeba853cc34e7dd471f16b4114f4162dc03e6b7afcc2128711f0eca823c \ + --hash=sha256:0150219816b6a1fa26fb4699fb7daa9caf09eb1999f3b70fb6e786805e80375a \ + --hash=sha256:02893d100e99e03eda1c8fd5c441d8c60103fd175728e23e431db1b589cf5ab3 \ + --hash=sha256:02ea2dfa234451bbb8772601d7b8e426c2bfa197136796224e50e35a78777956 \ + --hash=sha256:0f29edc409a6392443abf94b9cf89ce99889a1dd5376d94316ae5145dfedd5d6 \ + --hash=sha256:10892704fc220243f5305762e276552a0395f7beb4dbf9b14ec8fd43b57f126c \ + --hash=sha256:16249ee61e95f858e83976573de0f5b2893b3677ba71c9dd36b9cf8be9ac6d65 \ + --hash=sha256:1d37d57ad971609cf3c53ba6a7e365e40660e3be0e5175fa9f2365a379d6095a \ + --hash=sha256:1ebe39cb5fc479422b83de611d14e2c0d3bb2a18bbcb01f229ab3cfbd8fee7a0 \ + --hash=sha256:214ed4befebe12df36bcc8bc2b64b396ca31be9304b8f59e25c11cf94a4c033b \ + --hash=sha256:2283a07e2c21a2aa78d9c4442724ec1eb15f5e42a723b99cb3d822d48f5f7ad1 \ + --hash=sha256:22ba7cfcad58ef3ecddc7ed1db3409af68d023b7f940da23c6c2a1890976eda6 \ + --hash=sha256:27c0abcb4a5dac13684a37f76e701e054692a9b2d3064b70f5e4eb54810553d7 \ + --hash=sha256:28c8d926f98f432f88adc23edf2e6d4921ac26fb084b028c733d01868d19007e \ + --hash=sha256:2e71d11abed7344e42a8849600193d15b6def118602c4c176f748e4583246007 \ + --hash=sha256:34d5fcd24b8445fadc33f9cf348c1047101756fd760b4dacb5c3e99755703310 \ + --hash=sha256:37503bfbfc9d2c40b344d06b2199cf0e96e97957ab1c1b546fd4f87e53e5d3e4 \ + --hash=sha256:3c5677e12444c15717b902a5798264fa7909e41153cdf9ef7ad571b704a63dd9 \ + --hash=sha256:3ff07ec89bae51176c0549bc4c63aa6202991da2d9a6129d7aef7f1407d3f295 \ + --hash=sha256:41715c910c881bc081f1e8872880d3c650acf13dfa8214bad49ed4cede7c34ea \ + --hash=sha256:418cf3f2111bc80e0933b2cd8cd04f286338bb88bdc7bc8e6dd775ebde60b5e0 \ + --hash=sha256:44edc647873928551a01e7a563d7452ccdebee747728c1080d881d68af7b997e \ + --hash=sha256:4a2e8cebe2ff6ab7d1050ecd59c25d4c8bd7e6f400f5f82b96557ac0abafd0ac \ + --hash=sha256:4ad1906908f2f5ae4e5a8ddfce73c320c2a1429ec52eafd27138b7f1cbe341c9 \ + --hash=sha256:501a031947e3a9025ed4405a168e6ef5ae3126c59f90ce0cd6f2bfc477be31b7 \ + --hash=sha256:5190d403f121660ce8d1d2c1bb2ef1bd05b5f68533fc5c2ea899bd15f4399b35 \ + --hash=sha256:5498cd1645aa724a7c71c8f378eb29ebe23da2fc0d7a08071d89469bf1d2defb \ + --hash=sha256:5cf4e27da7e3fbed4d6c3d8e797387aaad68102272f8f9752883bc32d61cb87b \ + --hash=sha256:5e0b74767e5f8c593e8c9b5912019159ed0533c70051e9cce3e8b6aa699fcd69 \ + --hash=sha256:5ed875a24292240029e4483f9d4a4b8a1ae08843b9c54f43fcc11e404532a8a5 \ + --hash=sha256:5fcd34e47f6e0b794d17de1b4ff496c00986e1c83f7ab2fb8fcfe9616ff7477b \ + --hash=sha256:5fdec68f91a0c6739b380c83b951e2c72ac0197ace422360e6d5a959d8d97b2c \ + --hash=sha256:6344df0d5755a2c9a276d4473ae6b90647e216ab4757f8426893b5dd2ac3f369 \ + --hash=sha256:64386e5e707d03a7e172c0701abfb7e10f0fb753ee1d773128192742712a98fd \ + --hash=sha256:652cb6edd41e718550aad172851962662ff2681490a8a711af6a4d288dd96824 \ + --hash=sha256:66291b10affd76d76f54fad28e22e51719ef9ba22b29e1d7d03d6777a9174198 \ + --hash=sha256:66e1674c3ef6f541c35191caae2d429b967b99e02040f5ba928632d9a7f0f065 \ + --hash=sha256:6adc77889b628398debc7b65c073bcb99c4a0237b248cacaf3fe8a557563ef6c \ + --hash=sha256:79005a0d97d5ddabfeeea4cf676af11e647e41d81c9a7722a193022accdb6b7c \ + --hash=sha256:7c6610def4f163542a622a73fb39f534f8c101d690126992300bf3207eab9764 \ + --hash=sha256:7f047e29dcae44602496db43be01ad42fc6f1cc0d8cd6c83d342306c32270196 \ + --hash=sha256:8098f252adfa6c80ab48096053f512f2321f0b998f98150cea9bd23d83e1467b \ + --hash=sha256:850774a7879607d3a6f50d36d04f00ee69e7fc816450e5f7e58d7f17f1ae5c00 \ + --hash=sha256:8d1fab6bb153a416f9aeb4b8763bc0f22a5586065f86f7664fc23339fc1c1fac \ + --hash=sha256:8da9669d359f02c0b91ccc01cac4a67f16afec0dac22c2ad09f46bee0697eba8 \ + --hash=sha256:8dc52c23056b9ddd46818a57b78404882310fb473d63f17b07d5c40421e47f8e \ + --hash=sha256:9149cad251584d5fb4981be1ecde53a1ca46c891a79788c0df828d2f166bda28 \ + --hash=sha256:93dda82c9c22deb0a405ea4dc5f2d0cda384168e466364dec6255b293923b2f3 \ + --hash=sha256:96b533f0e99f6579b3d4d4995707cf36df9100d67e0c8303a0c55b27b5f99bc5 \ + --hash=sha256:9c57bb8c96f6d1808c030b1687b9b5fb476abaa47f0db9c0101f5e9f394e97f4 \ + --hash=sha256:9c7708761fccb9397fe64bbc0395abcae8c4bf7b0eac081e12b809bf47700d0b \ + --hash=sha256:9f3bfb4965eb874431221a3ff3fdcddc7e74e3b07799e0e84ca4a0f867d449bf \ + --hash=sha256:a33284e20b78bd4a18c8c2282d549d10bc8408a2a7ff57653c0cf0b9be0afce5 \ + --hash=sha256:a80cb027f6b349846a3bf6d73b5e95e782175e52f22108cfa17876aaeff93702 \ + --hash=sha256:b30236e45cf30d2b8e7b3e85881719e98507abed1011bf463a8fa23e9c3e98a8 \ + --hash=sha256:b3bc83488de33889877a0f2543ade9f70c67d66d9ebb4ac959502e12de895788 \ + --hash=sha256:b865addae83924361678b652338317d1bd7e79b1f4596f96b96c77a5a34b34da \ + --hash=sha256:b8bb0864c5a28024fac8a632c443c87c5aa6f215c0b126c449ae1a150412f31d \ + --hash=sha256:ba1cc08a7ccde2d2ec775841541641e4548226580ab850948cbfda66a1befcdc \ + --hash=sha256:bdb2c67c6c1390b63c6ff89f210c8fd09d9a1217a465701eac7316313c915e4c \ + --hash=sha256:c1ff362665ae507275af2853520967820d9124984e0f7466736aea23d8611fba \ + --hash=sha256:c2514fceb77bc5e7a2f7adfaa1feb2fb311607c9cb518dbc378688ec73d8292f \ + --hash=sha256:c3355370a2c156cffb25e876646f149d5d68f5e0a3ce86a5084dd0b64a994917 \ + --hash=sha256:c458b6d084f9b935061bc36216e8a69a7e293a2f1e68bf956dcd9e6cbcd143f5 \ + --hash=sha256:d0eae10f8159e8fdad514efdc92d74fd8d682c933a6dd088030f3834bc8e6b26 \ + --hash=sha256:d76623373421df22fb4cf8817020cbb7ef15c725b9d5e45f17e189bfc384190f \ + --hash=sha256:ebc55a14a21cb14062aa4162f906cd962b28e2e9ea38f9b4391244cd8de4ae0b \ + --hash=sha256:eda16858a3cab07b80edaf74336ece1f986ba330fdb8ee0d6c0d68fe82bc96be \ + --hash=sha256:ee2922902c45ae8ccada2c5b501ab86c36525b883eff4255313a253a3160861c \ + --hash=sha256:efd7b85f94a6f21e4932043973a7ba2613b059c4a000551892ac9f1d11f5baf3 \ + --hash=sha256:f7057c9a337546edc7973c0d3ba84ddcdf0daa14533c2065749c9075001090e6 \ + --hash=sha256:fa160448684b4e94d80416c0fa4aac48967a969efe22931448d853ada8baf926 \ + --hash=sha256:fc09d0aa354569bc501d4e787133afc08552722d3ab34836a80547331bb5d4a0 + # via ansible-core +resolvelib==1.0.1 \ + --hash=sha256:04ce76cbd63fded2078ce224785da6ecd42b9564b1390793f64ddecbe997b309 \ + --hash=sha256:d2da45d1a8dfee81bdd591647783e340ef3bcb104b54c383f70d422ef5cc7dbf + # via ansible-core diff --git a/src/software/embedded/motor_controller/BUILD b/src/software/embedded/motor_controller/BUILD index 8e1b1aa90c..27f6eedf59 100644 --- a/src/software/embedded/motor_controller/BUILD +++ b/src/software/embedded/motor_controller/BUILD @@ -67,9 +67,9 @@ cc_binary( ], deps = [ ":motor_controller", + "//software/embedded/services:uart_communicator", "//software/logger", "//software/logger:network_logger", - "//software/embedded/services:uart_communicator", "@boost//:program_options", ], ) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 67e967b089..cfa6dc5296 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -22,7 +22,8 @@ using Crc8Autosar = crc_utils::crc; StSpinMotorController::StSpinMotorController( - const robot_constants::RobotConstants& robot_constants, std::shared_ptr uart) + const robot_constants::RobotConstants& robot_constants, + std::shared_ptr uart) : robot_constants_(robot_constants), reset_gpio_(std::make_unique(RESET_GPIO_PIN, GpioDirection::OUTPUT, GpioState::HIGH)), diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index b288a1e836..a46d799aa9 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -18,7 +18,7 @@ class StSpinMotorController : public MotorController { public: explicit StSpinMotorController(const robot_constants::RobotConstants& robot_constants, - std::shared_ptr power_service); + std::shared_ptr power_service); void setup() override; 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 dd9d26c186..c2f71bbf2b 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -6,9 +6,9 @@ #include #include +#include "software/embedded/services/uart_communicator.h" #include "software/logger/logger.h" #include "software/logger/network_logger.h" -#include "software/embedded/services/uart_communicator.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; @@ -46,7 +46,7 @@ class StSpinMotorControllerTest ss << motor << " "; } LOG(INFO) << "Enabled motors: " << ss.str(); - const auto uart =std::make_shared(); + const auto uart = std::make_shared(); const auto motor_controller = std::make_unique( robot_constants::createRobotConstants(), uart); motor_controller->setup(); diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index 787122ba4a..19cc76cf51 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -40,7 +40,7 @@ cc_library( hdrs = ["imu.h"], deps = [ "//proto:tbots_cc_proto", - "//shared:constants", + "//shared:constants", "//software/geom:angular_acceleration", "//software/geom:angular_velocity", "//software/logger", @@ -58,5 +58,5 @@ cc_library( "//software/uart:boost_uart_communication", "@boost//:asio", "@boost//:filesystem", - ] -) \ No newline at end of file + ], +) diff --git a/src/software/embedded/services/power.cpp b/src/software/embedded/services/power.cpp index e7361cdc64..5899f17d89 100644 --- a/src/software/embedded/services/power.cpp +++ b/src/software/embedded/services/power.cpp @@ -5,14 +5,11 @@ #include "proto/power_frame_msg.pb.h" -PowerService::PowerService(std::shared_ptr uart): uart_(uart) -{ - -} +PowerService::PowerService(std::shared_ptr uart) : uart_(uart) {} TbotsProto::PowerStatus PowerService::poll(const TbotsProto::PowerControl& command, double kick_coeff, int kick_constant, int chip_constant) { return uart_->sendChipKickCommand(command, kick_coeff, kick_constant, chip_constant); -} \ No newline at end of file +} diff --git a/src/software/embedded/services/uart_communicator.cpp b/src/software/embedded/services/uart_communicator.cpp index 532d16ed62..651d5d7f27 100644 --- a/src/software/embedded/services/uart_communicator.cpp +++ b/src/software/embedded/services/uart_communicator.cpp @@ -30,9 +30,9 @@ void UartCommunicator::sendDribbleTarget(int rpm) _new_dribble_command = true; } -TbotsProto::PowerStatus UartCommunicator::sendChipKickCommand(const TbotsProto::PowerControl& command, - double kick_coeff, int kick_constant, - int chip_constant) +TbotsProto::PowerStatus UartCommunicator::sendChipKickCommand( + const TbotsProto::PowerControl& command, double kick_coeff, int kick_constant, + int chip_constant) { // Store msg for later transmission nanopb_command = @@ -99,4 +99,4 @@ void UartCommunicator::continuousRead() { tick(); } -} \ No newline at end of file +} diff --git a/src/software/embedded/services/uart_communicator.h b/src/software/embedded/services/uart_communicator.h index 70c27fa1e8..a6137f3898 100644 --- a/src/software/embedded/services/uart_communicator.h +++ b/src/software/embedded/services/uart_communicator.h @@ -16,7 +16,7 @@ extern "C" /** * Communicates to the powerboard over uart. */ -class UartCommunicator +class UartCommunicator { public: /** @@ -34,7 +34,8 @@ class UartCommunicator * @return the latest power status */ TbotsProto::PowerStatus sendChipKickCommand(const TbotsProto::PowerControl& control, - double kick_coeff, int kick_constant, int chip_constant); + double kick_coeff, int kick_constant, + int chip_constant); /** * Set dribbler RPM @@ -68,4 +69,4 @@ class UartCommunicator bool _new_dribble_command = false; std::atomic dribble_command; -}; \ No newline at end of file +}; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index f03a109053..65598e3f01 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -23,16 +23,18 @@ #include "software/embedded/motor_controller/stspin_motor_controller.h" #include "software/embedded/motor_controller/tmc_motor_controller.h" #include "software/embedded/services/uart_communicator.h" -======= + == == == + = #include "software/world/robot_state.h" ->>>>>>> 08b84cf832ecd330f4c775d10982d11be540d474 + >>>>>>> 08b84cf832ecd330f4c775d10982d11be540d474 -/** - * https://web.archive.org/web/20210308013218/https://rt.wiki.kernel.org/index.php/Squarewave-example - * using clock_nanosleep of librt - */ -extern int clock_nanosleep(clockid_t __clock_id, int __flags, - __const struct timespec* __req, struct timespec* __rem); + /** + * https://web.archive.org/web/20210308013218/https://rt.wiki.kernel.org/index.php/Squarewave-example + * using clock_nanosleep of librt + */ + extern int + clock_nanosleep(clockid_t __clock_id, int __flags, __const struct timespec* __req, + struct timespec* __rem); // signal handling is done by csignal which requires a function pointer with C linkage extern "C" @@ -125,17 +127,20 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, << "THUNDERLOOP: Network Service initialized! Next initializing Power Service"; std::shared_ptr uart = std::make_shared(); - power_service_ = std::make_unique(uart); - LOG(INFO) - << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; + power_service_ = std::make_unique(uart); + LOG(INFO) + << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; if constexpr (MOTOR_BOARD == MotorBoard::TRINAMIC) { - motor_service_ = std::make_unique(robot_constants,std::make_unique()); + motor_service_ = std::make_unique( + robot_constants, std::make_unique()); } else { - motor_service_ = std::make_unique(robot_constants, std::make_unique(robot_constants, uart)); + motor_service_ = std::make_unique( + robot_constants, + std::make_unique(robot_constants, uart)); } g_motor_service = motor_service_.get(); motor_service_->setup(); From 944356c165c74cc414cf7b7a5b9637dec1131bfd Mon Sep 17 00:00:00 2001 From: Grayson Date: Thu, 4 Jun 2026 23:51:15 -0700 Subject: [PATCH 33/34] set pin --- src/software/embedded/thunderloop.cpp | 3 --- src/software/power/pins.h | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index f03a109053..7a8a33fa1d 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -18,14 +18,11 @@ #include "software/networking/tbots_network_exception.h" #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" -<<<<<<< HEAD #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/embedded/services/uart_communicator.h" -======= #include "software/world/robot_state.h" ->>>>>>> 08b84cf832ecd330f4c775d10982d11be540d474 /** * https://web.archive.org/web/20210308013218/https://rt.wiki.kernel.org/index.php/Squarewave-example diff --git a/src/software/power/pins.h b/src/software/power/pins.h index bc958d251c..b5eb47e324 100644 --- a/src/software/power/pins.h +++ b/src/software/power/pins.h @@ -32,7 +32,7 @@ const uint8_t PM_SDA = 13; const uint8_t PM_SCL = 14; // Dribbler -const uint8_t DRIBBLER_PIN = 0; // When elec has a pin for this +const uint8_t DRIBBLER_PIN = 4; // When elec has a pin for this // Timers const uint8_t CHICKER_PULSE_TIMER = 0; From 69d4c9621aa0f8590408816c8eb10b12ba87ec4d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Fri, 5 Jun 2026 06:58:02 +0000 Subject: [PATCH 34/34] [pre-commit.ci lite] apply automatic fixes --- src/software/embedded/thunderloop.cpp | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 9929c08cf7..71c666d431 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -10,27 +10,26 @@ #include "proto/tbots_software_msgs.pb.h" #include "shared/constants.h" #include "software/constants.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/embedded/primitive_executor.h" #include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" +#include "software/embedded/services/uart_communicator.h" #include "software/logger/logger.h" #include "software/logger/network_logger.h" #include "software/networking/tbots_network_exception.h" #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.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/embedded/services/uart_communicator.h" #include "software/world/robot_state.h" - /** - * https://web.archive.org/web/20210308013218/https://rt.wiki.kernel.org/index.php/Squarewave-example - * using clock_nanosleep of librt - */ - extern int - clock_nanosleep(clockid_t __clock_id, int __flags, __const struct timespec* __req, - struct timespec* __rem); +/** + * https://web.archive.org/web/20210308013218/https://rt.wiki.kernel.org/index.php/Squarewave-example + * using clock_nanosleep of librt + */ +extern int clock_nanosleep(clockid_t __clock_id, int __flags, + __const struct timespec* __req, struct timespec* __rem); // signal handling is done by csignal which requires a function pointer with C linkage extern "C"