add MP Scara type and arms visualisation

src/MarlinSimulator/hardware/KinematicSystem.cpp

@@ -413,3 +413,97 @@ void DeltaKinematicSystem::ui_widget() {
   ImGui::Text("y: %f", state.effector_position[0].position.y);
   ImGui::Text("z: %f", state.effector_position[0].position.z);
 }
+
+#if ENABLED(MP_SCARA)
+
+ScaraKinematicSystem::ScaraKinematicSystem(std::function<void(kinematic_state&)> on_kinematic_update) : KinematicSystem(on_kinematic_update) {
+  collect_steppers();
+  // if defined MP_SCARA it means degrees of arms when nozzle is at (0,0,z) position
+  // when SCARA_LINKAGE_1 = 250mm, SCARA_LINKAGE_2 = 250mm, SCARA_OFFSET_X = 75mm, SCARA_OFFSET_Y = -100mm
+  // you can calculate the degrees with following method which is copyed from Marlin-bugfix-2.1.x
+  /*
+  static constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y  };
+  float constexpr L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2;
+  void inverse_kinematics(const xyz_pos_t &raw) {
+        // Translate SCARA to standard XY with scaling factor
+    const xy_pos_t spos = raw - scara_offset;
+    const float x = spos.x, y = spos.y, c = HYPOT(x, y),
+                THETA3 = ATAN2(y, x),
+                THETA1 = THETA3 + ACOS((sq(c) + sq(L1) - sq(L2)) / (2.0f * c * L1)),
+                THETA2 = THETA3 - ACOS((sq(c) + sq(L2) - sq(L1)) / (2.0f * c * L2));
+
+    delta.set(DEGREES(THETA1), DEGREES(THETA2), raw.z);
+  }
+  */
+  hardware_offset.push_back({ 202.39,  51.35, 0.0 });
+}
+
+glm::vec3 ScaraKinematicSystem::forward_kinematics(const double a, const double b, const double z) {
+    const float a_sin = std::sin(glm::radians(a)) * SCARA_LINKAGE_1,
+                a_cos = std::cos(glm::radians(a)) * SCARA_LINKAGE_1,
+                b_sin = std::sin(glm::radians(b)) * SCARA_LINKAGE_2,
+                b_cos = std::cos(glm::radians(b)) * SCARA_LINKAGE_2;
+
+  // return the pos to the position of Tower
+
+  return glm::vec3{ a_cos + b_cos + SCARA_OFFSET_X,
+                    a_sin + b_sin + SCARA_OFFSET_Y,
+                    z};
+}
+
+void ScaraKinematicSystem::kinematic_update() {
+  auto carriage = glm::vec3{
+    std::static_pointer_cast<StepperDriver>(steppers[AxisIndex::X])->steps() / steps_per_unit[0] * (((INVERT_X_DIR * 2) - 1) * -1.0),
+    std::static_pointer_cast<StepperDriver>(steppers[AxisIndex::Y])->steps() / steps_per_unit[1] * (((INVERT_Y_DIR * 2) - 1) * -1.0),
+    std::static_pointer_cast<StepperDriver>(steppers[AxisIndex::Z])->steps() / steps_per_unit[2] * (((INVERT_Z_DIR * 2) - 1) * -1.0)
+  };
+
+  extruder.clear();
+  for (size_t i = 0; i < EXTRUDERS; ++i) {
+    extruder.push_back(std::static_pointer_cast<StepperDriver>(steppers[AxisIndex::E0 + i])->steps() / steps_per_unit[3 + (i * distinct_e_factors)] * (((extruder_invert_dir[i] * 2) - 1) * -1.0));
+  }
+  double a = hardware_offset[0].x + carriage.x;
+  double b = hardware_offset[0].y + carriage.y;
+  auto cartesian_pos = forward_kinematics(a, b, hardware_offset[0].z + carriage.z);
+
+
+  for (size_t i = 0; i < HOTENDS; ++i) {
+    state.effector_position[i] = {carriage, glm::vec4(cartesian_pos, extruder[i]), filament_color[i]};
+  }
+
+  state.position = state.effector_position[0].position;
+  state.arm_angle.clear();
+  state.arm_angle.push_back(a);
+  state.arm_angle.push_back(b);
+  on_kinematic_update(state);
+}
+
+void ScaraKinematicSystem::ui_widget() {
+  if (state.effector_position.size() > 0) {
+    auto value = hardware_offset[0].x + state.effector_position[0].stepper_position.x;
+    if (ImGui::SliderFloat("hardware(a) offset (deg)", &value, -360,  360)) {
+      hardware_offset[0].x = value - state.effector_position[0].stepper_position.x;
+      kinematic_update();
+    }
+    value = hardware_offset[0].y + state.effector_position[0].stepper_position.y;
+    if (ImGui::SliderFloat("hardware(b) offset (deg)",  &value, -360,  360)) {
+      hardware_offset[0].y = value - state.effector_position[0].stepper_position.y;
+      kinematic_update();
+    }
+    value = hardware_offset[0].z + state.effector_position[0].stepper_position.z;
+    if (ImGui::SliderFloat("hardware(z) offset (mm)",  &value, -10,  100)) {
+      hardware_offset[0].z = value - state.effector_position[0].stepper_position.z;
+      kinematic_update();
+    }
+    ImGui::Text("Stepper Position:");
+    ImGui::Text("x: %f", state.effector_position[0].stepper_position.x);
+    ImGui::Text("y: %f", state.effector_position[0].stepper_position.y);
+    ImGui::Text("z: %f", state.effector_position[0].stepper_position.z);
+    ImGui::Text("Cartesian Position:");
+    ImGui::Text("x: %f", state.effector_position[0].position.x);
+    ImGui::Text("y: %f", state.effector_position[0].position.y);
+    ImGui::Text("z: %f", state.effector_position[0].position.z);
+  }
+}
+
+#endif

src/MarlinSimulator/hardware/KinematicSystem.h

@@ -3,7 +3,6 @@
 #include <memory>
 #include <atomic>
 #include <glm/glm.hpp>
-
 #include <imgui.h>
 
 #include "Gpio.h"
@@ -19,6 +18,9 @@ struct extruder_state {
 struct kinematic_state {
   std::vector<extruder_state> effector_position {};
   glm::vec3 position {};
+#if ENABLED(MP_SCARA)
+  std::vector<double> arm_angle {};
+#endif
 };
 
 class KinematicSystem : public VirtualPrinter::Component {
@@ -60,3 +62,15 @@ class DeltaKinematicSystem : public KinematicSystem {
   glm::vec3 forward_kinematics(const double z1, const double z2, const double z3);
   void recalc_delta_settings();
 };
+
+#if ENABLED(MP_SCARA)
+class ScaraKinematicSystem : public KinematicSystem {
+public:
+  ScaraKinematicSystem(std::function<void(kinematic_state&)> on_kinematic_update);
+  virtual void ui_widget() override;
+  virtual void kinematic_update() override;
+  glm::vec3 forward_kinematics(const double a, const double b, const double z);
+  std::vector<glm::vec3> hardware_offset {};
+  std::vector<double> extruder {};
+};
+#endif

src/MarlinSimulator/hardware/ScaraArm.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <imgui.h>
+
+#include "../virtual_printer.h"
+
+class ScaraArm : public VirtualPrinter::Component {
+public:
+  ScaraArm() : VirtualPrinter::Component("ScaraArm") {
+
+  }
+  ~ScaraArm() {}
+
+  void ui_widget() {
+    ImGui::Checkbox("Enabled", &enabled);
+  }
+
+  bool enabled = true;
+};

src/MarlinSimulator/virtual_printer.cpp

@@ -16,6 +16,7 @@
 #include "hardware/NeoPixelDevice.h"
 #include "hardware/KinematicSystem.h"
 #include "hardware/pwm_reader.h"
+#include "hardware/ScaraArm.h"
 
 #include "virtual_printer.h"
 
@@ -26,6 +27,10 @@
   #include MARLIN_HAL_PATH(tft/xpt2046.h)
 #endif
 
+#ifndef SD_DETECT_PIN
+  #define SD_DETECT_PIN -1
+#endif
+
 #ifndef SD_DETECT_STATE
   #define SD_DETECT_STATE HIGH
 #endif
@@ -51,7 +56,10 @@ void VirtualPrinter::Component::ui_widgets() {
 void VirtualPrinter::build() {
   root = add_component<Component>("root");
 
-  #if ENABLED(DELTA)
+  #if ENABLED(MP_SCARA)
+    auto kinematics = root->add_component<ScaraKinematicSystem>("MP Scara Kinematic System", on_kinematic_update);
+    root->add_component<ScaraArm>("ScaraArm");
+  #elif ENABLED(DELTA)
     auto kinematics = root->add_component<DeltaKinematicSystem>("Delta Kinematic System", on_kinematic_update);
     root->add_component<EndStop>("Endstop(Tower A Max)", X_MAX_PIN, !X_MAX_ENDSTOP_HIT_STATE, [kinematics](){ return kinematics->state.effector_position[0].stepper_position.x >= DELTA_HEIGHT; });
     root->add_component<EndStop>("Endstop(Tower B Max)", Y_MAX_PIN, !Y_MAX_ENDSTOP_HIT_STATE, [kinematics](){ return kinematics->state.effector_position[0].stepper_position.y >= DELTA_HEIGHT; });

src/MarlinSimulator/visualisation.cpp

@@ -19,17 +19,84 @@ Visualisation::Visualisation(VirtualPrinter& virtual_printer) : virtual_printer(
     for (size_t i = 0; i < state.effector_position.size(); ++i) {
       this->set_head_position(i, state.effector_position[i]);
     }
+  #if ENABLED(MP_SCARA)
+    if(state.arm_angle.size() == 2){
+      arm_angle_changed = false;
+      for (size_t i = 0; i < state.arm_angle.size(); ++i) {
+        if(this->arm_angle[i] != state.arm_angle[i])
+          this->arm_angle[i] = state.arm_angle[i];
+          arm_angle_changed = true;
+      }      
+    }
+  #endif
   };
 
   for (int i = 0; i < EXTRUDERS; ++i) {
     extrusion.push_back({});
   }
+#if ENABLED(MP_SCARA)
+  arm_angle.push_back({});
+  arm_angle.push_back({});
+#endif
 }
 
 Visualisation::~Visualisation() {
   destroy();
 }
 
+renderer::mesh_id_t Visualisation::scara_add_mesh_arm(const double link_length)
+{
+  auto mesh_arm = renderer::create_mesh();
+  auto buffer_arm = renderer::Buffer<renderer::vertex_data_t>::create();
+  float arm_L = link_length;
+  float arm_W = arm_L/100;
+  float arm_H = arm_L/100;
+  float x = -arm_W/2;
+  float y = arm_H/2;
+  float z = arm_L;
+
+  buffer_arm->data() = {
+      //top 
+      renderer::vertex_data_t EFFECTOR_VERTEX(0, y, x, EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(0, y, -x,  EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(z, y, -x, EFFECTOR_COLOR_2),
+
+      EFFECTOR_VERTEX(z, y, x, EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(0, y, x,  EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(z, y, -x, EFFECTOR_COLOR_2),
+
+      //left
+      EFFECTOR_VERTEX(0, -y, x, EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(0, y, x,  EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(z, y, x, EFFECTOR_COLOR_2),
+
+      EFFECTOR_VERTEX(z, y, x, EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(z, -y, x,  EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(0, -y, x, EFFECTOR_COLOR_2),
+
+      //right
+      EFFECTOR_VERTEX(0, y, -x, EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(0, -y, -x,  EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(z, -y, -x, EFFECTOR_COLOR_2),
+
+      EFFECTOR_VERTEX(z, -y, -x, EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(z, y, -x,  EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(0, y, -x, EFFECTOR_COLOR_2),
+
+      //front
+      EFFECTOR_VERTEX(0, y, x, EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(0, -y, x,  EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(0, -y, -x, EFFECTOR_COLOR_2),
+
+      EFFECTOR_VERTEX(0, -y, -x, EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(0, -y, x,  EFFECTOR_COLOR_2),
+      EFFECTOR_VERTEX(0, y, x, EFFECTOR_COLOR_2)
+  };
+  renderer::get_mesh_by_id(mesh_arm)->buffer_vector<renderer::vertex_data_t>().push_back(buffer_arm);
+  renderer::get_mesh_by_id(mesh_arm)->set_shader_program(default_program);
+  return mesh_arm;
+}
+
 void Visualisation::create() {
   extrusion_program = renderer::ShaderProgram::create("data/shaders/extrusion.vs", "data/shaders/extrusion.fs", "data/shaders/extrusion.gs");
   default_program = renderer::ShaderProgram::create("data/shaders/default.vs","data/shaders/default.fs");
@@ -83,7 +150,11 @@ void Visualisation::create() {
     mesh_object->set_shader_program(default_program);
     mesh_object->m_scale = effector_scale;
   }
-
+#if ENABLED(MP_SCARA)
+// add scara arms
+  m_arm_mesh.push_back(scara_add_mesh_arm(SCARA_LINKAGE_1));
+  m_arm_mesh.push_back(scara_add_mesh_arm(SCARA_LINKAGE_2));  
+#endif
   m_bed_mesh = renderer::create_mesh();
   auto mesh_object = renderer::get_mesh_by_id(m_bed_mesh);
   mesh_object->set_shader_program(default_program);
@@ -123,6 +194,7 @@ void Visualisation::create() {
 
   auto kin = virtual_printer.get_component<KinematicSystem>("Cartesian Kinematic System");
   if(kin == nullptr) kin = virtual_printer.get_component<KinematicSystem>("Delta Kinematic System");
+  if(kin == nullptr) kin = virtual_printer.get_component<KinematicSystem>("MP Scara Kinematic System");
   if (kin != nullptr && kin->state.effector_position.size() == extrusion.size()) {
     size_t i = 0;
     for (auto state : kin->state.effector_position) {
@@ -137,6 +209,11 @@ void Visualisation::create() {
   for (auto mesh : m_extruder_mesh) {
     renderer::render_mesh(mesh);
   }
+#if ENABLED(MP_SCARA)
+  for (auto mesh : m_arm_mesh) {
+    renderer::render_mesh(mesh);
+  }
+#endif
   renderer::render_list_is_ready();
   m_initialised = true;
 }
@@ -234,12 +311,43 @@ void Visualisation::update() {
 
     mesh_id ++;
   }
-
+#if ENABLED(MP_SCARA)
+  auto scara_arm = virtual_printer.get_component<ScaraArm>("ScaraArm");
+  // update the position of the arm mesh for visualisation
+  if(m_arm_mesh.size()>0 && arm_angle_changed)
+  {
+    int arm_index = 0;
+    for (auto it = m_arm_mesh.begin(); it != m_arm_mesh.end(); ++it)
+    {
+      auto mesh_object = renderer::get_mesh_by_id(*it);
+      if(arm_index == 0)
+      {
+        mesh_object->m_position = glm::vec3{SCARA_OFFSET_X, effector_pos.y, -SCARA_OFFSET_Y};;
+      }
+      else if(arm_index == 1)
+      {
+        const float a_sin = std::sin(glm::radians( arm_angle[0])) * SCARA_LINKAGE_1,
+                    a_cos = std::cos(glm::radians(arm_angle[0])) * SCARA_LINKAGE_1;
+        mesh_object->m_position = glm::vec3{a_cos + SCARA_OFFSET_X, effector_pos.y, -(a_sin + SCARA_OFFSET_Y)};
+      }
+      glm::qua<float> m_rotation = glm::qua<float>(glm::radians(glm::vec3(0.0f, arm_angle[arm_index], 0.0f)));
+      mesh_object->m_rotation = m_rotation;
+      mesh_object->m_transform_dirty = true;
+      mesh_object->m_visible = (follow_mode != FOLLOW_Z)&&scara_arm->enabled;
+      arm_index++;
+    }
+  }
+#endif
   if (draw_list_update) {
     renderer::render_mesh(m_bed_mesh);
     for (auto mesh : m_extruder_mesh) {
       renderer::render_mesh(mesh);
     }
+#if ENABLED(MP_SCARA)
+    for (auto mesh : m_arm_mesh) {
+      renderer::render_mesh(mesh);
+    }
+#endif    
     for (auto& ext : extrusion) {
       renderer::render_mesh(ext.mesh);
     }

src/MarlinSimulator/visualisation.h

@@ -5,6 +5,7 @@
 
 #include "hardware/print_bed.h"
 #include "hardware/bed_probe.h"
+#include "hardware/ScaraArm.h"
 #include "hardware/KinematicSystem.h"
 
 #include <gl.h>
@@ -137,11 +138,15 @@ class Visualisation {
   void ui_info_callback(UiWindow*);
 
   std::vector<Extrusion> extrusion {};
+#if ENABLED(MP_SCARA)
+  std::vector<float> arm_angle {};
+  bool arm_angle_changed = false;
+#endif
   std::mutex extrusion_mutex {};
 
   void set_head_position(size_t hotend_index, extruder_state& position);
   bool points_are_collinear(const glm::vec3 a, const glm::vec3 b, const glm::vec3 c, double const threshold) const;
-
+  renderer::mesh_id_t scara_add_mesh_arm(const double link_length);
   FollowMode follow_mode = FOLLOW_NONE;
   bool render_full_path = true;
   bool render_path_line = false;
@@ -154,6 +159,9 @@ class Visualisation {
   PerspectiveCamera camera;
   opengl_util::FrameBuffer* framebuffer = nullptr;
   std::vector<renderer::mesh_id_t> m_extruder_mesh;
+#if ENABLED(MP_SCARA)
+  std::vector<renderer::mesh_id_t> m_arm_mesh;
+#endif
   renderer::mesh_id_t m_bed_mesh;
 
   std::shared_ptr<renderer::ShaderProgram> extrusion_program;