feat(thermocycler-refresh): Add Peltier closed-loop control (#230)

* Added commands to set the Plate Temperature and deactivate the plate temperature control * Peltiers are controlled via PID if a temperature is currently set * Added tests for the commands * Added handling in the Plate Task for the PID settings message * Added tests for lid heater & plate to make sure outputs are being set in control loop * Fixed a bug where peltier outputs driving full power heating would do nothing, because the PWM ends up being 0 after inverting it * Added a script for basic temperature cycling for testing * Added error checking when updating peltier PID outputs
Opentrons · Dec 9, 2021 · 3a995e3 · 3a995e3
1 parent 5f84bf9
commit 3a995e3
Show file tree

Hide file tree

Showing 13 changed files with 1,129 additions and 13 deletions.
diff --git a/stm32-modules/include/thermocycler-refresh/thermocycler-refresh/gcodes.hpp b/stm32-modules/include/thermocycler-refresh/thermocycler-refresh/gcodes.hpp
@@ -679,6 +679,96 @@ struct DeactivateLidHeating {
     }
 };
 
+struct SetPlateTemperature {
+    /**
+     * SetPlateTemperature uses M104. Parameters:
+     * - S - setpoint temperature
+     * - H - hold time (optional)
+     *
+     * M104 S44\n
+     */
+    using ParseResult = std::optional<SetPlateTemperature>;
+    static constexpr auto prefix = std::array{'M', '1', '0', '4', ' ', 'S'};
+    static constexpr auto hold_prefix = std::array{' ', 'H'};
+    static constexpr const char* response = "M104 OK\n";
+
+    // 0 seconds means infinite hold time
+    constexpr static double infinite_hold = 0.0F;
+
+    double setpoint;
+    double hold_time;
+
+    template <typename InputIt, typename InLimit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<InputIt, InLimit>
+    static auto write_response_into(InputIt buf, InLimit limit) -> InputIt {
+        return write_string_to_iterpair(buf, limit, response);
+    }
+
+    template <typename InputIt, typename Limit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<Limit, InputIt>
+    static auto parse(const InputIt& input, Limit limit)
+        -> std::pair<ParseResult, InputIt> {
+        auto working = prefix_matches(input, limit, prefix);
+        if (working == input) {
+            return std::make_pair(ParseResult(), input);
+        }
+        // We are expecting a temperature setting
+        auto temperature = parse_value<float>(working, limit);
+        if (!temperature.first.has_value()) {
+            return std::make_pair(ParseResult(), input);
+        }
+        auto temperature_val = temperature.first.value();
+
+        auto hold_val = infinite_hold;
+        working = prefix_matches(temperature.second, limit, hold_prefix);
+        if (working != temperature.second) {
+            // This command specified a hold temperature
+            auto hold = parse_value<float>(working, limit);
+            if (!hold.first.has_value()) {
+                return std::make_pair(ParseResult(), input);
+            }
+            hold_val = hold.first.value();
+            working = hold.second;
+        }
+
+        return std::make_pair(
+            ParseResult(SetPlateTemperature{.setpoint = temperature_val,
+                                            .hold_time = hold_val}),
+            working);
+    }
+};
+
+struct DeactivatePlate {
+    /**
+     * DeactivatePlate uses M14. It has no parameters and just
+     * deactivates the plate peltiers + fan.
+     */
+    using ParseResult = std::optional<DeactivatePlate>;
+    static constexpr auto prefix = std::array{'M', '1', '4'};
+    static constexpr const char* response = "M14 OK\n";
+
+    template <typename InputIt, typename InLimit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<InputIt, InLimit>
+    static auto write_response_into(InputIt buf, InLimit limit) -> InputIt {
+        return write_string_to_iterpair(buf, limit, response);
+    }
+
+    template <typename InputIt, typename Limit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<Limit, InputIt>
+    static auto parse(const InputIt& input, Limit limit)
+        -> std::pair<ParseResult, InputIt> {
+        auto working = prefix_matches(input, limit, prefix);
+        if (working == input) {
+            return std::make_pair(ParseResult(), input);
+        }
+        return std::make_pair(ParseResult(DeactivatePlate()), working);
+    }
+};
+
 struct SetPIDConstants {
     /**
      * SetPIDConstants uses M301. It has three parameters, along with

diff --git a/stm32-modules/include/thermocycler-refresh/thermocycler-refresh/host_comms_task.hpp b/stm32-modules/include/thermocycler-refresh/thermocycler-refresh/host_comms_task.hpp
@@ -42,12 +42,14 @@ class HostCommsTask {
         gcode::GetLidTemperatureDebug, gcode::GetPlateTemperatureDebug,
         gcode::SetPeltierDebug, gcode::SetFanManual, gcode::SetHeaterDebug,
         gcode::GetPlateTemp, gcode::GetLidTemp, gcode::SetLidTemperature,
-        gcode::DeactivateLidHeating, gcode::SetPIDConstants>;
+        gcode::DeactivateLidHeating, gcode::SetPIDConstants,
+        gcode::SetPlateTemperature, gcode::DeactivatePlate>;
     using AckOnlyCache =
         AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber,
                  gcode::SetPeltierDebug, gcode::SetFanManual,
                  gcode::SetHeaterDebug, gcode::SetLidTemperature,
-                 gcode::DeactivateLidHeating, gcode::SetPIDConstants>;
+                 gcode::DeactivateLidHeating, gcode::SetPIDConstants,
+                 gcode::SetPlateTemperature, gcode::DeactivatePlate>;
     using GetSystemInfoCache = AckCache<8, gcode::GetSystemInfo>;
     using GetLidTempDebugCache = AckCache<8, gcode::GetLidTemperatureDebug>;
     using GetPlateTempDebugCache = AckCache<8, gcode::GetPlateTemperatureDebug>;
@@ -708,9 +710,63 @@ class HostCommsTask {
                                              .p = gcode.const_p,
                                              .i = gcode.const_i,
                                              .d = gcode.const_d};
-        // TODO when PID is added to the peltiers and fans, will have to
-        // switch the target queue based on the selection in the message.
-        if (!task_registry->lid_heater->get_message_queue().try_send(
+        bool ret = false;
+        if (message.selection == PidSelection::HEATER) {
+            ret = task_registry->lid_heater->get_message_queue().try_send(
+                message, TICKS_TO_WAIT_ON_SEND);
+        } else {
+            ret = task_registry->thermal_plate->get_message_queue().try_send(
+                message, TICKS_TO_WAIT_ON_SEND);
+        }
+        if (!ret) {
+            auto wrote_to = errors::write_into(
+                tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
+            ack_only_cache.remove_if_present(id);
+            return std::make_pair(false, wrote_to);
+        }
+
+        return std::make_pair(true, tx_into);
+    }
+
+    template <typename InputIt, typename InputLimit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<InputLimit, InputIt>
+    auto visit_gcode(const gcode::SetPlateTemperature& gcode, InputIt tx_into,
+                     InputLimit tx_limit) -> std::pair<bool, InputIt> {
+        auto id = ack_only_cache.add(gcode);
+        if (id == 0) {
+            return std::make_pair(
+                false, errors::write_into(tx_into, tx_limit,
+                                          errors::ErrorCode::GCODE_CACHE_FULL));
+        }
+
+        auto message = messages::SetPlateTemperatureMessage{
+            .id = id, .setpoint = gcode.setpoint, .hold_time = gcode.hold_time};
+        if (!task_registry->thermal_plate->get_message_queue().try_send(
+                message, TICKS_TO_WAIT_ON_SEND)) {
+            auto wrote_to = errors::write_into(
+                tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
+            ack_only_cache.remove_if_present(id);
+            return std::make_pair(false, wrote_to);
+        }
+
+        return std::make_pair(true, tx_into);
+    }
+
+    template <typename InputIt, typename InputLimit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<InputLimit, InputIt>
+    auto visit_gcode(const gcode::DeactivatePlate& gcode, InputIt tx_into,
+                     InputLimit tx_limit) -> std::pair<bool, InputIt> {
+        auto id = ack_only_cache.add(gcode);
+        if (id == 0) {
+            return std::make_pair(
+                false, errors::write_into(tx_into, tx_limit,
+                                          errors::ErrorCode::GCODE_CACHE_FULL));
+        }
+
+        auto message = messages::DeactivatePlateMessage{.id = id};
+        if (!task_registry->thermal_plate->get_message_queue().try_send(
                 message, TICKS_TO_WAIT_ON_SEND)) {
             auto wrote_to = errors::write_into(
                 tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);

diff --git a/stm32-modules/include/thermocycler-refresh/thermocycler-refresh/messages.hpp b/stm32-modules/include/thermocycler-refresh/thermocycler-refresh/messages.hpp
@@ -174,6 +174,16 @@ struct DeactivateLidHeatingMessage {
     uint32_t id;
 };
 
+struct SetPlateTemperatureMessage {
+    uint32_t id;
+    double setpoint;
+    double hold_time;
+};
+
+struct DeactivatePlateMessage {
+    uint32_t id;
+};
+
 struct SetPIDConstantsMessage {
     uint32_t id;
     PidSelection selection;
@@ -194,7 +204,9 @@ using HostCommsMessage =
 using ThermalPlateMessage =
     ::std::variant<std::monostate, ThermalPlateTempReadComplete,
                    GetPlateTemperatureDebugMessage, SetPeltierDebugMessage,
-                   SetFanManualMessage, GetPlateTempMessage>;
+                   SetFanManualMessage, GetPlateTempMessage,
+                   SetPlateTemperatureMessage, DeactivatePlateMessage,
+                   SetPIDConstantsMessage>;
 using LidHeaterMessage =
     ::std::variant<std::monostate, LidTempReadComplete,
                    GetLidTemperatureDebugMessage, SetHeaterDebugMessage,