fix(flex-stacker): more evt qc fixes (#487)

* fix ebrake engage & disengage delay issue * fix rsense and parse stop motor message
Opentrons · Nov 27, 2024 · ed0094d · ed0094d
1 parent 1587809
commit ed0094d
Show file tree

Hide file tree

Showing 8 changed files with 35 additions and 14 deletions.
diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c b/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c
@@ -3,6 +3,8 @@
 #include "motor_hardware.h"
 #include "systemwide.h"
 #include "FreeRTOS.h"
+#include "task.h"
+
 
 #include <stdbool.h>
 #include <stdint.h>
@@ -313,7 +315,11 @@ void hw_enable_ebrake(MotorID motor_id, bool enable) {
     if (motor_id != MOTOR_Z) {
         return;
     }
+    if (!enable) {
+        vTaskDelay(1);
+    }
     HAL_GPIO_WritePin(Z_N_BRAKE_PORT, Z_N_BRAKE_PIN, enable ? GPIO_PIN_RESET : GPIO_PIN_SET);
+    vTaskDelay(70);
     return;
 }
 
@@ -340,17 +346,21 @@ void reset_pin(PinConfig config) {
 
 bool hw_enable_motor(MotorID motor_id) {
     stepper_hardware_t motor = get_motor(motor_id);
-    hw_enable_ebrake(motor_id, false);
     HAL_StatusTypeDef status = HAL_OK;
-    status = HAL_TIM_Base_Start_IT(&motor.timer);
     set_pin(motor.enable);
+    hw_enable_ebrake(motor_id, false);
     return status == HAL_OK;
 }
 
+void hw_start_motor_timer(MotorID motor_id) {
+    stepper_hardware_t motor = get_motor(motor_id);
+    HAL_TIM_Base_Start_IT(&motor.timer);
+}
+
 bool hw_disable_motor(MotorID motor_id) {
     stepper_hardware_t motor = get_motor(motor_id);
-    reset_pin(motor.enable);
     hw_enable_ebrake(motor_id, true);
+    reset_pin(motor.enable);
     return true;
 }
 

diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp b/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp
@@ -9,6 +9,11 @@ auto MotorPolicy::enable_motor(MotorID motor_id) -> bool {
     return hw_enable_motor(motor_id);
 }
 
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto MotorPolicy::start_motor_timer(MotorID motor_id) -> void {
+    hw_start_motor_timer(motor_id);
+}
+
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
 auto MotorPolicy::disable_motor(MotorID motor_id) -> bool {
     return hw_disable_motor(motor_id);

diff --git a/stm32-modules/include/flex-stacker/firmware/motor_hardware.h b/stm32-modules/include/flex-stacker/firmware/motor_hardware.h
@@ -16,6 +16,7 @@ bool motor_spi_sendreceive(MotorID motor_id, uint8_t *tx_data, uint8_t *rx_data,
 
 void hw_step_motor(MotorID motor_id);
 bool hw_enable_motor(MotorID motor_id);
+void hw_start_motor_timer(MotorID motor_id);
 bool hw_disable_motor(MotorID motor_id);
 bool hw_stop_motor(MotorID motor_id);
 void hw_set_direction(MotorID, bool direction);

diff --git a/stm32-modules/include/flex-stacker/firmware/motor_interrupt.hpp b/stm32-modules/include/flex-stacker/firmware/motor_interrupt.hpp
@@ -47,9 +47,6 @@ class MotorInterruptController {
         }
         if (ret.done || stop_condition_met()) {
             _policy->stop_motor(_id);
-            if (_id == MotorID::MOTOR_Z) {
-                _policy->disable_motor(_id);
-            }
             _stop = true;
             return true;
         }
@@ -74,6 +71,7 @@ class MotorInterruptController {
     auto start_move(Move move) -> void {
         motor_util::MotorState* state = move.motor_state;
         _stop = false;
+        _policy->enable_motor(_id);
         set_direction(move.direction);
         _profile = motor_util::MovementProfile(
             TIMER_FREQ, state->get_speed_discont(),
@@ -83,12 +81,15 @@ class MotorInterruptController {
             move.limit_switch ? motor_util::MovementType::OpenLoop
                               : motor_util::MovementType::FixedDistance,
             state->get_distance(move.distance));
-        _policy->enable_motor(_id);
         _response_id = move.move_id;
+        _policy->start_motor_timer(_id);
     }
     auto stop_movement(uint32_t move_id, bool disable_motor) -> void {
         _stop = true;
-        disable_motor ? _policy->disable_motor(_id) : _policy->stop_motor(_id);
+        _policy->stop_motor(_id);
+        if (disable_motor) {
+            _policy->disable_motor(_id);
+        }
         _response_id = move_id;
     }
 

diff --git a/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp b/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp
@@ -9,6 +9,7 @@ namespace motor_policy {
 class MotorPolicy {
   public:
     auto enable_motor(MotorID motor_id) -> bool;
+    auto start_motor_timer(MotorID motor_id) -> void;
     auto disable_motor(MotorID motor_id) -> bool;
     auto stop_motor(MotorID motor_id) -> bool;
     auto step(MotorID motor_id) -> void;

diff --git a/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp
@@ -47,14 +47,15 @@ class HostCommsTask {
         gcode::MoveMotorInSteps, gcode::MoveToLimitSwitch, gcode::MoveMotorInMm,
         gcode::GetLimitSwitches, gcode::SetMicrosteps, gcode::GetMoveParams,
         gcode::SetMotorStallGuard, gcode::GetMotorStallGuard, gcode::HomeMotor,
-        gcode::GetPlatformSensors, gcode::GetDoorClosed, gcode::GetEstopStatus>;
+        gcode::GetPlatformSensors, gcode::GetDoorClosed, gcode::GetEstopStatus,
+        gcode::StopMotor>;
     using AckOnlyCache =
         AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber,
                  gcode::SetTMCRegister, gcode::SetRunCurrent,
                  gcode::SetHoldCurrent, gcode::EnableMotor, gcode::DisableMotor,
                  gcode::MoveMotorInSteps, gcode::MoveToLimitSwitch,
                  gcode::MoveMotorInMm, gcode::SetMicrosteps,
-                 gcode::SetMotorStallGuard, gcode::HomeMotor>;
+                 gcode::SetMotorStallGuard, gcode::HomeMotor, gcode::StopMotor>;
     using GetSystemInfoCache = AckCache<8, gcode::GetSystemInfo>;
     using GetTMCRegisterCache = AckCache<8, gcode::GetTMCRegister>;
     using GetLimitSwitchesCache = AckCache<8, gcode::GetLimitSwitches>;
@@ -479,7 +480,7 @@ class HostCommsTask {
         if (!task_registry->send(message, TICKS_TO_WAIT_ON_SEND)) {
             auto wrote_to = errors::write_into(
                 tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
-            get_system_info_cache.remove_if_present(id);
+            ack_only_cache.remove_if_present(id);
             return std::make_pair(false, wrote_to);
         }
         return std::make_pair(true, tx_into);
@@ -500,7 +501,7 @@ class HostCommsTask {
         if (!task_registry->send(message, TICKS_TO_WAIT_ON_SEND)) {
             auto wrote_to = errors::write_into(
                 tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
-            get_system_info_cache.remove_if_present(id);
+            ack_only_cache.remove_if_present(id);
             return std::make_pair(false, wrote_to);
         }
         return std::make_pair(true, tx_into);

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
@@ -343,7 +343,7 @@ class MotorDriverTask {
     tmc2160::TMC2160 _tmc2160{};
     // same motor current config for all three motors
     const tmc2160::TMC2160MotorCurrentConfig _motor_current_config{
-        .r_sense = 0.22,
+        .r_sense = 0.15,
         .v_sf = 0.325,
     };
     tmc2160::TMC2160RegisterMap _x_config = motor_x_config;

diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp
@@ -360,12 +360,14 @@ class MotorTask {
     template <MotorControlPolicy Policy>
     auto visit_message(const messages::MoveCompleteMessage& m, Policy& policy)
         -> void {
-        static_cast<void>(policy);
         Move next_move;
         if (_move_queue.dequeue(next_move)) {
             // if there's a next move in the queue, start it
             controller_from_id(next_move.motor_id).start_move(next_move);
         } else {
+            if (m.motor_id == MotorID::MOTOR_Z) {
+                policy.disable_motor(m.motor_id);
+            }
             send_ack_message(controller_from_id(m.motor_id).get_response_id());
         }
     }