ok i think this works

Opentrons · Jul 25, 2024 · edff483 · edff483
1 parent fcfc8cb
commit edff483
Show file tree

Hide file tree

Showing 12 changed files with 306 additions and 285 deletions.
diff --git a/stm32-modules/flex-stacker/firmware/CMakeLists.txt b/stm32-modules/flex-stacker/firmware/CMakeLists.txt
@@ -17,9 +17,9 @@ set(GDBINIT_PATH "${CMAKE_CURRENT_BINARY_DIR}/../../common/STM32G491/gdbinit")
 set(${TARGET_MODULE_NAME}_FW_LINTABLE_SRCS
     ${CMAKE_CURRENT_SOURCE_DIR}/main.cpp
     ${SYSTEM_DIR}/freertos_idle_timer_task.cpp
-    ${MOTOR_CONTROL_DIR}/freertos_motor_driver_task.cpp
     ${MOTOR_CONTROL_DIR}/freertos_motor_task.cpp
-    ${MOTOR_CONTROL_DIR}/motor_policy.cpp
+    ${MOTOR_CONTROL_DIR}/freertos_motor_driver_task.cpp
+    ${MOTOR_CONTROL_DIR}/motor_driver_policy.cpp
     )
 
 # Add source files that should NOT be checked by clang-tidy here

diff --git a/stm32-modules/flex-stacker/firmware/main.cpp b/stm32-modules/flex-stacker/firmware/main.cpp
@@ -17,16 +17,17 @@ static auto aggregator = tasks::FirmwareTasks::QueueAggregator();
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto driver_task =
-    ot_utils::freertos_task::FreeRTOSTask<tasks::MOTOR_DRIVER_STACK_SIZE, EntryPoint>(
-        motor_driver_task_entry);
+    ot_utils::freertos_task::FreeRTOSTask<tasks::MOTOR_DRIVER_STACK_SIZE,
+                                          EntryPoint>(motor_driver_task_entry);
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto motor_task =
     ot_utils::freertos_task::FreeRTOSTask<tasks::MOTOR_STACK_SIZE, EntryPoint>(
         motor_task_entry);
 
 auto main() -> int {
     HardwareInit();
-    driver_task.start(tasks::MOTOR_DRIVER_TASK_PRIORITY, "Motor Driver", &aggregator);
+    driver_task.start(tasks::MOTOR_DRIVER_TASK_PRIORITY, "Motor Driver",
+                      &aggregator);
     motor_task.start(tasks::MOTOR_TASK_PRIORITY, "Motor", &aggregator);
 
     vTaskStartScheduler();

diff --git a/stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_driver_task.cpp b/stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_driver_task.cpp
@@ -1,8 +1,7 @@
-#include "firmware/freertos_tasks.hpp"
-
 #include "FreeRTOS.h"
-#include "firmware/motor_hardware.h"
+#include "firmware/freertos_tasks.hpp"
 #include "firmware/motor_driver_policy.hpp"
+#include "firmware/motor_hardware.h"
 #include "flex-stacker/motor_driver_task.hpp"
 #include "ot_utils/freertos/freertos_timer.hpp"
 

diff --git a/stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_task.cpp b/stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_task.cpp
@@ -1,6 +1,5 @@
-#include "firmware/freertos_tasks.hpp"
-
 #include "FreeRTOS.h"
+#include "firmware/freertos_tasks.hpp"
 #include "firmware/motor_hardware.h"
 #include "firmware/motor_policy.hpp"
 #include "flex-stacker/motor_task.hpp"
@@ -27,7 +26,7 @@ auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void {
     aggregator->register_queue(_queue);
     _top_task.provide_aggregator(aggregator);
 
-//        motor_hardware_init();
+    //        motor_hardware_init();
 
     //    auto policy = motor_policy::MotorPolicy();
     while (true) {

diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_driver_policy.cpp b/stm32-modules/flex-stacker/firmware/motor_control/motor_driver_policy.cpp
@@ -2,11 +2,15 @@
 
 #include "firmware/motor_hardware.h"
 
+using namespace motor_driver_policy;
 
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
-auto MotorDriverPolicy::tmc2160_transmit_receive(MotorID motor_id, tmc2160::MessageT& data) -> RxTxReturn {
+auto MotorDriverPolicy::tmc2160_transmit_receive(MotorID motor_id,
+                                                 tmc2160::MessageT& data)
+    -> RxTxReturn {
     tmc2160::MessageT retBuf = {0};
-    if (spi_dma_transmit_receive(motor_id, data.data(), retBuf.data(), data.size())) {
+    if (spi_dma_transmit_receive(motor_id, data.data(), retBuf.data(),
+                                 data.size())) {
         return RxTxReturn(retBuf);
     }
     return RxTxReturn();

diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_spi_hardware.c b/stm32-modules/flex-stacker/firmware/motor_control/motor_spi_hardware.c
@@ -39,7 +39,8 @@ struct motor_spi_hardware {
 };
 
 static void spi_interrupt_service(void);
-static void spi_set_nss(bool selected);
+static void enable_spi_nss(MotorID motor);
+static void disable_spi_nss(void);
 
 
 DMA_HandleTypeDef hdma_spi2_rx;
@@ -284,11 +285,11 @@ void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi)
     spi_interrupt_service();
 }
 
-bool tmc2160_transmit_receive(
+bool spi_dma_transmit_receive(
     MotorID motor_id, uint8_t *txData, uint8_t *rxData, uint16_t size
 ) {
     const TickType_t max_block_time = pdMS_TO_TICKS(100);
-    HAL_StatusTypeDef ret;
+//    HAL_StatusTypeDef ret;
     uint32_t notification_val = 0;
 
     if (!_spi.initialized || (_spi.task_to_notify != NULL) || (size > MOTOR_MAX_SPI_LEN)) {

diff --git a/stm32-modules/include/flex-stacker/firmware/motor_driver_policy.hpp b/stm32-modules/include/flex-stacker/firmware/motor_driver_policy.hpp
@@ -2,14 +2,16 @@
 
 #include <optional>
 
+#include "flex-stacker/tmc2160_interface.hpp"
 #include "systemwide.h"
-#include "flex-stacker/tmc2160.hpp"
 
 namespace motor_driver_policy {
+
 class MotorDriverPolicy {
   public:
     using RxTxReturn = std::optional<tmc2160::MessageT>;
-    auto tmc2160_transmit_receive(MotorID motor_id, tmc2160::MessageT& data) -> RxTxReturn;
+    auto tmc2160_transmit_receive(MotorID motor_id, tmc2160::MessageT& data)
+        -> RxTxReturn;
 };
 
 }  // namespace motor_driver_policy
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp
@@ -127,10 +127,10 @@ using SystemMessage =
     ::std::variant<std::monostate, AcknowledgePrevious, GetSystemInfoMessage,
                    SetSerialNumberMessage, EnterBootloaderMessage>;
 
- using MotorDriverMessage = ::std::variant<std::monostate, SetMotorDriverRegister,
-                                    GetMotorDriverRegister,
-                                    PollMotorDriverRegister,
-                                    StopPollingMotorDriverRegister>;
+using MotorDriverMessage =
+    ::std::variant<std::monostate, SetMotorDriverRegister,
+                   GetMotorDriverRegister, PollMotorDriverRegister,
+                   StopPollingMotorDriverRegister>;
 using MotorMessage = ::std::variant<std::monostate>;
 
 };  // namespace messages
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_driver_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_driver_task.hpp
@@ -5,8 +5,8 @@
  */
 #pragma once
 
+#include <optional>
 
-#include "systemwide.h"
 #include "core/ack_cache.hpp"
 #include "core/queue_aggregator.hpp"
 #include "core/version.hpp"
@@ -17,13 +17,12 @@
 #include "flex-stacker/tmc2160_interface.hpp"
 #include "flex-stacker/tmc2160_registers.hpp"
 #include "hal/message_queue.hpp"
+#include "systemwide.h"
 
 namespace motor_driver_task {
 
-
 using Message = messages::MotorDriverMessage;
 
-
 static constexpr tmc2160::TMC2160RegisterMap motor_z_config{
     .gconfig = {.diag0_error = 1, .diag1_stall = 1},
     .glob_scale = {.global_scaler = 0x8B},
@@ -70,7 +69,7 @@ static constexpr tmc2160::TMC2160RegisterMap motor_x_config{
                 .pwm_lim = 0xC},
 };
 
-constexpr tmc2160::TMC2160RegisterMap motor_l_config{
+static constexpr tmc2160::TMC2160RegisterMap motor_l_config{
     .gconfig = {.diag0_error = 1, .diag1_stall = 1},
     .glob_scale = {.global_scaler = 0x8B},
     .ihold_irun = {.hold_current = 1,
@@ -107,7 +106,8 @@ class MotorDriverTask {
     MotorDriverTask(const MotorDriverTask& other) = delete;
     auto operator=(const MotorDriverTask& other) -> MotorDriverTask& = delete;
     MotorDriverTask(MotorDriverTask&& other) noexcept = delete;
-    auto operator=(MotorDriverTask&& other) noexcept -> MotorDriverTask& = delete;
+    auto operator=(MotorDriverTask&& other) noexcept
+        -> MotorDriverTask& = delete;
     ~MotorDriverTask() = default;
 
     auto provide_aggregator(Aggregator* aggregator) {
@@ -120,16 +120,22 @@ class MotorDriverTask {
             return;
         }
         if (!_initialized) {
-
-            if (!_tmc2160.initialize_config(motor_z_config, tmc2160::TMC2160Interface(policy), MotorID::MOTOR_Z)) {
+            if (!_tmc2160.hello()) {
                 return;
             }
-            if (!_tmc2160.initialize_config(motor_x_config, tmc2160::TMC2160Interface(policy), MotorID::MOTOR_X)) {
-                return;
-            }
-            if (!_tmc2160.initialize_config(motor_l_config, tmc2160::TMC2160Interface(policy), MotorID::MOTOR_L)) {
+            static tmc2160::TMC2160Interface<Policy> tmc2160_interface(policy);
+            if (!_tmc2160.initialize_config(motor_z_config, tmc2160_interface,
+                                            MotorID::MOTOR_Z)) {
                 return;
             }
+            //            if (!_tmc2160.initialize_config(motor_x_config,
+            //            _tmc2160_interface, MotorID::MOTOR_X)) {
+            //                return;
+            //            }
+            //            if (!_tmc2160.initialize_config(motor_l_config,
+            //            _tmc2160_interface, MotorID::MOTOR_L)) {
+            //                return;
+            //            }
             _initialized = true;
         }
 
@@ -147,31 +153,30 @@ class MotorDriverTask {
         static_cast<void>(message);
     }
 
-
-    auto visit_message(const messages::SetMotorDriverRegister& message) -> void {
+    auto visit_message(const messages::SetMotorDriverRegister& message)
+        -> void {
         static_cast<void>(message);
     }
 
-    auto visit_message(const messages::GetMotorDriverRegister& message) -> void {
+    auto visit_message(const messages::GetMotorDriverRegister& message)
+        -> void {
         static_cast<void>(message);
     }
 
-    auto visit_message(const messages::PollMotorDriverRegister& message) -> void {
+    auto visit_message(const messages::PollMotorDriverRegister& message)
+        -> void {
         static_cast<void>(message);
     }
 
-    auto visit_message(const messages::StopPollingMotorDriverRegister& message) -> void {
+    auto visit_message(const messages::StopPollingMotorDriverRegister& message)
+        -> void {
         static_cast<void>(message);
     }
 
-
-
     Queue& _message_queue;
     Aggregator* _task_registry;
     bool _initialized;
 
     tmc2160::TMC2160 _tmc2160{};
-    tmc2160::TMC2160Interface<Policy>* _tmc2160_interface;
-
 };
-};  // namespace motor_task
+};  // namespace motor_driver_task
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/tasks.hpp b/stm32-modules/include/flex-stacker/flex-stacker/tasks.hpp
@@ -16,9 +16,9 @@ struct Tasks {
     // Message queue for motor control task
     using MotorQueue = QueueImpl<messages::MotorMessage>;
 
-
     // Central aggregator
-    using QueueAggregator = queue_aggregator::QueueAggregator<MotorDriverQueue, MotorQueue>;
+    using QueueAggregator =
+        queue_aggregator::QueueAggregator<MotorDriverQueue, MotorQueue>;
 
     // Addresses
     static constexpr size_t MotorDriverAddress =