Implement STSPIN motor controller and refactor motor service by williamckha · Pull Request #3715 · UBC-Thunderbots/Software

williamckha · 2026-05-18T05:21:00Z

Description

Testing Done

Resolved Issues

Resolves #3711

Length Justification and Key Files to Review

Review Checklist

It is the reviewers responsibility to also make sure every item here has been covered

Function & Class comments: All function definitions (usually in the .h file) should have a javadoc style comment at the start of them. For examples, see the functions defined in thunderbots/software/geom. Similarly, all classes should have an associated Javadoc comment explaining the purpose of the class.
Remove all commented out code
Remove extra print statements: for example, those just used for testing
Resolve all TODO's: All TODO (or similar) statements should either be completed or associated with a github issue

williamckha · 2026-05-18T20:06:37Z

+void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len,
+                 uint32_t spi_speed);


Refactor these SPI transfer functions to take std::array as params instead of raw pointers

williamckha · 2026-05-18T20:12:01Z

+    using OutgoingFrame =
+        std::variant<NoOpFrame, SetResponseTypeFrame, SetTargetSpeedFrame,
+                     SetTargetTorqueFrame, SetPidTorqueKpKiFrame, SetPidFluxKpKiFrame,
+                     SetPidSpeedKpKiFrame, SetSpeedFeedForwardKaKvFrame,
+                     SetSpeedFeedForwardKsFrame>;


Instead of using std::variant and visitor in processTx, consider turning processTx into a template method with frame type as a template param, and use template specialization to select implementation based on frame type

williamckha · 2026-05-18T20:13:37Z

        << "THUNDERLOOP: Network Service initialized! Next initializing Power Service";

-    power_service_ = std::make_unique<PowerService>();
+    // power_service_ = std::make_unique<PowerService>();


Reenable power service before this is merged into master

williamckha · 2026-05-18T20:17:17Z


 const std::string SSL_VISION_ADDRESS          = "224.5.23.2";
-static constexpr unsigned int SSL_VISION_PORT = 10020;
+static constexpr unsigned int SSL_VISION_PORT = 10006;


This is the correct default SSL vision port

williamckha · 2026-05-18T20:20:08Z

+constexpr std::array<MotorIndex, 4> driveMotors()
+{
+    return {
+        MotorIndex::FRONT_LEFT,
+        MotorIndex::FRONT_RIGHT,
+        MotorIndex::BACK_LEFT,
+        MotorIndex::BACK_RIGHT,
+    };
+}


Might be better to make this a global variable instead of a function returning a new array every time. This isn't great because e.g. driveMotors().begin() and driveMotors().end() will return iterators to different arrays

williamckha · 2026-05-18T20:22:03Z

+    /*********** 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;


Kind of a hacky solution just appending these new fault types to our MotorFault enum. It would be better if we had separate enums for Trinamic vs. STSPIN faults

williamckha · 2026-05-18T20:23:44Z

+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();
+        const auto elapsed_time = current_time - start_time;
+        if (elapsed_time > timeout_ms)
+        {
+            return false;
+        }
+    }
+    return true;
+}


Should use steady_clock instead of system_clock because system_clock is not always monotonically increasing

williamckha · 2026-05-18T20:25:43Z

+    logWorker->addSink(std::make_unique<PlotJugglerSink>("tbotswifi5"),
+                       &PlotJugglerSink::sendToPlotJuggler);


It would be nice if we could change the network interface used for the PlotJugglerSink at runtime and set it to whatever interface we've configured in the robot_config.toml file

Created #3718, can you complete the accept criteria? I am not 100% sure I understand what you want here. Am I right to imagine that you want the sink to be added dynamically at startup after the contents of the toml file are transferred over the network to the device running thunderscope/logger? Or, is this logger running on thunderloop itself and thus the contents of the toml do not need to be forwarded

NetworkLogger runs on the robot, i.e. as part of thunderloop. I don't think you can add sinks after glog is initialized (which happens before the toml config is loaded), but we can add a method to the plotjuggler sink that lets us reconfigure the network interface used + reopen socket, and then assuming we hold a reference to the plotjuggler sink, call this method after we've fetched the network interface from the toml config.

Andrewyx

Left some comments on the scope of some parts. We can either merge this one in and then refactor over it through subtasks, or remove the unrelated portions and have subtasks begin cleanly.

Andrewyx · 2026-05-18T20:44:23Z

-
-// Constants for the Raspberry Pi
-
-constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO = 16;


Add comments to these

Andrewyx · 2026-05-18T21:02:31Z

+    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;
+


Andrewyx · 2026-05-18T21:14:45Z

+    std::ostringstream oss;
+    oss << "======= Faults For Motor " << motor << "=======\n";
+
+    if (fault_flags & static_cast<uint16_t>(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<uint16_t>(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<uint16_t>(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<uint16_t>(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<uint16_t>(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<uint16_t>(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<uint16_t>(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<uint16_t>(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<uint16_t>(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<uint16_t>(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<uint16_t>(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.

We can probably just use string::append

Andrewyx · 2026-05-18T21:51:09Z

+    const Vector local_velocity(target_euclidean_velocity[1],
+                                -target_euclidean_velocity[0]);
+
+    if (local_velocity.length() <= 0.01)
+    {


nit: magic numbers

Andrewyx · 2026-05-18T21:55:55Z

+struct SetPidTorqueKpKiFrame
+{
+    int16_t kp;
+    int16_t ki;
+};
+
+struct SetPidFluxKpKiFrame
+{
+    int16_t kp;
+    int16_t ki;
+};
+
+struct SetPidSpeedKpKiFrame
+{
+    int16_t kp;
+    int16_t ki;
+};
+


Why do we need so many structs of the same underlying structure?

The type of frame changes how we populate the transmit buffer (mainly opcode at the start of the frame)

Andrewyx · 2026-05-18T22:20:44Z

        def stop_test(delay):
            time.sleep(delay)
-            if self.thunderscope:
-                self.thunderscope.close()
+            # We no longer close thunderscope here, because a test might call run_test multiple times.
+            # Thunderscope will be closed when the fixture is torn down.



Mark with todo, this function does nothing currently. We should close thunderscope after a while once field test completes or fails

Andrewyx · 2026-05-18T22:21:43Z

+    parser.add_argument(
+        "--no_visualization",
+        action="store_true",
+        default=False,
+        help="Disables the Thunderscope GUI",
+    )
+


this feature is probably unnecessary, since we almost always want a GUI

Andrewyx · 2026-05-18T22:55:16Z

-                id: tactic,
-            },
-            yellow_tactics=None,
+    def execute_test():


These changes are to fix field tests for the new gc changes for 2026. We should extract this to a separate PR. TODO #3717

Andrewyx · 2026-05-18T23:10:46Z

+    logWorker->addSink(std::make_unique<PlotJugglerSink>("tbotswifi5"),
+                       &PlotJugglerSink::sendToPlotJuggler);


Created #3718, can you complete the accept criteria? I am not 100% sure I understand what you want here. Am I right to imagine that you want the sink to be added dynamically at startup after the contents of the toml file are transferred over the network to the device running thunderscope/logger? Or, is this logger running on thunderloop itself and thus the contents of the toml do not need to be forwarded

Andrewyx · 2026-05-18T23:16:42Z

+        # State for drag-to-orient movement
+        self.is_dragging_to_orient = False
+        self.target_point = None
+        self.current_orientation = -math.pi / 2
+


Extract this work to #3719

The base branch was changed.

GrayHoang

comments for me to sort each file change

GrayHoang · 2026-05-22T22:25:31Z

Documentation/Testing (MD)

GrayHoang · 2026-05-22T22:26:22Z

robot constants and robot control

GrayHoang · 2026-05-22T22:26:34Z

robot constants and robot control

GrayHoang · 2026-05-22T22:26:48Z

robot constants and control

GrayHoang · 2026-05-22T22:27:00Z

robot constants and control

GrayHoang · 2026-05-22T22:55:39Z


 const std::string SSL_VISION_ADDRESS          = "224.5.23.2";
-static constexpr unsigned int SSL_VISION_PORT = 10020;
+static constexpr unsigned int SSL_VISION_PORT = 10006;


GrayHoang · 2026-05-22T22:55:56Z

robotconstants

GrayHoang · 2026-05-22T22:56:08Z

…into tbots/motor # Conflicts: # src/software/embedded/services/motor.cpp # src/software/embedded/services/motor.h # src/software/embedded/services/robot_auto_test.cpp # src/software/embedded/thunderloop.cpp # src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp

williamckha and others added 7 commits May 17, 2026 21:55

Clean up robot constants and make them selectable at compile time

ac62aaa

Refactor motor service and implement STSPIN motor controller

903ec93

Fix field tests

520c4f4

Run lint and format

89c7e0e

Fix build errors

1a2f397

Missed some changes for motor service

6938ccc

Update robot_status_msg.proto

e7e6931

williamckha requested review from Andrewyx and sauravbanna as code owners May 18, 2026 05:21

williamckha linked an issue May 18, 2026 that may be closed by this pull request

MDv6 Thunderloop Integration #3711

Open

1 task

Add documentation

0c92ab0

williamckha commented May 18, 2026

View reviewed changes

Andrewyx previously approved these changes May 18, 2026

View reviewed changes

williamckha changed the base branch from tbots/robot_constants to master May 20, 2026 00:51

GrayHoang reviewed May 22, 2026

View reviewed changes

GrayHoang mentioned this pull request May 23, 2026

Stspin Motor driver integration #3734

Open

4 tasks

williamckha and others added 2 commits May 22, 2026 18:30

[pre-commit.ci lite] apply automatic fixes

882875c

		void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len,
		uint32_t spi_speed);

		logWorker->addSink(std::make_unique<PlotJugglerSink>("tbotswifi5"),
		&PlotJugglerSink::sendToPlotJuggler);


		// Constants for the Raspberry Pi

		constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO = 16;

Conversation

williamckha commented May 18, 2026

Description

Testing Done

Resolved Issues

Length Justification and Key Files to Review

Review Checklist

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Andrewyx left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

GrayHoang left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects