add bltouch/servo support (#49)

Author: ellensp

src/MarlinSimulator/hardware/BLTouch.cpp

@@ -0,0 +1,27 @@
+#define BLTOUCH_DEPLOY          10
+#define BLTOUCH_STOW            90
+#define BLTOUCH_SW_MODE         60
+#define BLTOUCH_SELFTEST       120
+#define BLTOUCH_MODE_STORE     130
+#define BLTOUCH_5V_MODE        140
+#define BLTOUCH_OD_MODE        150
+#define BLTOUCH_RESET          160
+
+#include "print_bed.h"
+#include "bed_probe.h"
+#include "pwm_reader.h"
+
+#include "BLTouch.h"
+
+void BLTouchProbe::interrupt(GpioEvent& ev) {
+  if (ev.pin_id == m_servo_pin) {
+    auto angle = dutycycle_to_degrees(m_servo->pwm_duty);
+    if (is_close_enough(angle, BLTOUCH_DEPLOY)) {
+      m_probe->enabled = true;
+      m_deployed = true;
+    } else if (is_close_enough(angle, BLTOUCH_STOW)) {
+      m_probe->enabled = false;
+      m_deployed = false;
+    }
+  }
+}

src/MarlinSimulator/hardware/BLTouch.h

@@ -0,0 +1,41 @@
+#pragma once
+#include <imgui.h>
+#include "../virtual_printer.h"
+
+constexpr uint16_t cmap(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max) {
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+constexpr uint16_t dutycycle_to_degrees(uint16_t value) {
+  return cmap((float(value) / std::numeric_limits<uint16_t>::max()) * 20000, 540, 2400, 0, 180);
+}
+constexpr bool is_close_enough(uint16_t value, uint16_t target, uint16_t max_error = 1) {
+  return (value >= target - max_error) && (value <= target + max_error);
+}
+
+class BLTouchProbe : public VirtualPrinter::Component {
+public:
+  BLTouchProbe(pin_type servo_pin, pin_type probe_pin, glm::vec3 offset, glm::vec4& position, PrintBed& bed) : VirtualPrinter::Component("BLTouch") {
+    m_servo_pin = servo_pin;
+    m_servo = add_component<PWMReader>("PWM Control Signal", servo_pin);
+    m_probe = add_component<BedProbe>("Probe Signal", probe_pin, offset, position, bed);
+    Gpio::attach(servo_pin, [this](GpioEvent& event){ this->interrupt(event); });
+  }
+  virtual ~BLTouchProbe() {}
+
+  void ui_widget() {
+    if (m_deployed) {
+      ImGui::Text("Probe Deployed");
+    } else {
+      ImGui::Text("Probe Stowed");
+    }
+    ImGui::Text("Command Angle: %d", dutycycle_to_degrees(m_servo->pwm_duty));
+  }
+
+  void interrupt(GpioEvent& ev);
+
+private:
+  bool m_deployed = false;
+  pin_type m_servo_pin;
+  std::shared_ptr<PWMReader> m_servo;
+  std::shared_ptr<BedProbe> m_probe;
+};

src/MarlinSimulator/hardware/bed_probe.h

@@ -16,14 +16,19 @@ class BedProbe : public VirtualPrinter::Component {
 
   void ui_widget() {
     auto has_triggered = triggered();
-    ImGui::Checkbox("Triggered State", &has_triggered);
-    ImGui::Text("Nozzle Distance Above Bed: %f", position.z - bed.calculate_z({position.x + offset.x, position.y + offset.y}));
+    if (enabled) {
+      ImGui::Checkbox("Triggered State", &has_triggered);
+      ImGui::Text("Nozzle Distance Above Bed: %f", position.z - bed.calculate_z({position.x + offset.x, position.y + offset.y}));
+    } else {
+      ImGui::Text("Disabled");
+    }
   }
 
   bool triggered() {
-    return position.z <= bed.calculate_z({position.x + offset.x, position.y + offset.y}) - offset.z;
+    return enabled && position.z <= bed.calculate_z({position.x + offset.x, position.y + offset.y}) - offset.z;
   }
 
+  bool enabled = true;
   pin_type probe_pin;
   glm::vec3 offset;
   glm::vec4& position;

src/MarlinSimulator/hardware/pwm_reader.cpp

@@ -28,7 +28,7 @@ void PWMReader::ui_widget() {
       ImGui::Text("Software PWM (%.2fHz) Duty Cycle: %.2f%%", pwm_period ? float(Kernel::TimeControl::ONE_BILLION) / Kernel::TimeControl::ticksToNanos(pwm_period) : 0, pwm_period ? float(pwm_duty * 100) / pwm_period : 0);
       break;
     case Hardware:
-      ImGui::Text("Hardware PWM Duty Cycle : %.2f%%", pwm_period ? float(pwm_duty * 100) / pwm_period : 0);
+      ImGui::Text("Hardware PWM Duty Cycle : (%ld) %.2f%%", pwm_duty, pwm_period ? float(pwm_duty * 100) / pwm_period : 0);
       break;
   }
 }
@@ -48,7 +48,7 @@ void PWMReader::interrupt(GpioEvent& ev) {
     } else if ( ev.event == ev.SET_VALUE) { // Hardware PWM
       pwm_mode = Hardware;
       pwm_duty = Gpio::get_pin_value(pwm_pin);
-      pwm_period = 255;
+      pwm_period = std::numeric_limits<uint16_t>::max();
     }
   }
 }

src/MarlinSimulator/virtual_printer.cpp

@@ -1,11 +1,11 @@
+#include <algorithm>
 #include <imgui.h>
 
 #include "hardware/Button.h"
 #include "hardware/StepperDriver.h"
 #include "hardware/EndStop.h"
 #include "hardware/Heater.h"
 #include "hardware/print_bed.h"
-#include "hardware/print_bed.h"
 #include "hardware/bed_probe.h"
 #include "hardware/ST7796Device.h"
 #include "hardware/HD44780Device.h"
@@ -16,6 +16,7 @@
 #include "hardware/NeoPixelDevice.h"
 #include "hardware/KinematicSystem.h"
 #include "hardware/pwm_reader.h"
+#include "hardware/BLTouch.h"
 
 #include "virtual_printer.h"
 
@@ -48,6 +49,8 @@ void VirtualPrinter::Component::ui_widgets() {
   }
 }
 
+std::map<uint64_t, uint64_t> servo_pin_lookup { {0, SERVO0_PIN}, {1, SERVO1_PIN}, {2, SERVO2_PIN}, {3, SERVO3_PIN}};
+
 void VirtualPrinter::build() {
   root = add_component<Component>("root");
 
@@ -69,7 +72,11 @@ void VirtualPrinter::build() {
   auto print_bed = root->add_component<PrintBed>("Print Bed", glm::vec2{X_BED_SIZE, Y_BED_SIZE});
 
   #if HAS_BED_PROBE
-    root->add_component<BedProbe>("Probe",
+    #if ENABLED(BLTOUCH)
+      root->add_component<BLTouchProbe>("BLTouch", servo_pin_lookup[Z_PROBE_SERVO_NR],
+    #else
+      root->add_component<BedProbe>("Probe",
+    #endif
     #ifdef Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN
       Z_MIN_PIN
     #else
@@ -140,6 +147,24 @@ void VirtualPrinter::build() {
     root->add_component<PWMReader>("Fan0", FAN0_PIN);
   #endif
 
+  #ifdef Z_PROBE_SERVO_NR
+    #define SERVO_TEST(N) Z_PROBE_SERVO_NR != N
+  #else
+    #define SERVO_TEST(N) 1
+  #endif
+  #if NUM_SERVOS > 0 && SERVO_TEST(0)
+    root->add_component<PWMReader>("Servo0", SERVO0_PIN);
+  #endif
+  #if NUM_SERVOS > 1 && SERVO_TEST(1)
+    root->add_component<PWMReader>("Servo1", SERVO1_PIN);
+  #endif
+  #if NUM_SERVOS > 2 && SERVO_TEST(2)
+    root->add_component<PWMReader>("Servo2", SERVO2_PIN);
+  #endif
+  #if NUM_SERVOS > 3 && SERVO_TEST(3)
+    root->add_component<PWMReader>("Servo3", SERVO3_PIN);
+  #endif
+
   #if ENABLED(SPI_FLASH)
     root->add_component<W25QxxDevice>("SPI Flash", spi_bus_by_pins<SPI_FLASH_SCK_PIN, SPI_FLASH_MOSI_PIN, SPI_FLASH_MISO_PIN>(), SPI_FLASH_CS_PIN, SPI_FLASH_SIZE);
   #endif

src/MarlinSimulator/virtual_printer.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <algorithm>
 #include <string>
 #include <memory>
 #include <map>