Skip to content

Commit

Permalink
Fix delta support (#44)
Browse files Browse the repository at this point in the history
  • Loading branch information
@p3p
p3p authored Apr 29, 2024
1 parent 8b0c6e3 commit afe7683
Show file tree
Hide file tree
Showing 5 changed files with 108 additions and 83 deletions.
125 changes: 72 additions & 53 deletions src/MarlinSimulator/hardware/KinematicSystem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,15 +86,14 @@ enum AxisIndex {
#if EXTRUDERS > 7
E7,
#endif
COUNT
};

KinematicSystem::KinematicSystem(std::function<void(kinematic_state)> on_kinematic_update) : VirtualPrinter::Component("Kinematic System"), on_kinematic_update(on_kinematic_update) {

void KinematicSystem::collect_steppers() {
steppers.push_back(add_component<StepperDriver>("StepperX", X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, [this](){ this->kinematic_update(); }));
#if HAS_X2_STEPPER
steppers.push_back(add_component<StepperDriver>("StepperX2", X2_ENABLE_PIN, X2_DIR_PIN, X2_STEP_PIN, [this](){ this->kinematic_update(); }));
#endif

steppers.push_back(add_component<StepperDriver>("StepperY", Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, [this](){ this->kinematic_update(); }));
#if HAS_Y2_STEPPER
steppers.push_back(add_component<StepperDriver>("StepperY2", Y2_ENABLE_PIN, Y2_DIR_PIN, Y2_STEP_PIN, [this](){ this->kinematic_update(); }));
Expand Down Expand Up @@ -143,6 +142,10 @@ KinematicSystem::KinematicSystem(std::function<void(kinematic_state)> on_kinemat
steppers.push_back(add_component<StepperDriver>("StepperE7", E7_ENABLE_PIN, E7_DIR_PIN, E7_STEP_PIN, [this](){ this->kinematic_update(); }));
state.effector_position.push_back({});
#endif
}

CartesianKinematicSystem::CartesianKinematicSystem(std::function<void(kinematic_state)> on_kinematic_update) : KinematicSystem(on_kinematic_update) {
collect_steppers();

srand(time(0));
hardware_offset.push_back(glm::vec3{
Expand Down Expand Up @@ -190,7 +193,7 @@ std::array<glm::vec3, 8> filament_color {
{0.0, 0.0, 0.0}
};

void KinematicSystem::kinematic_update() {
void CartesianKinematicSystem::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),
Expand Down Expand Up @@ -224,7 +227,7 @@ void KinematicSystem::kinematic_update() {
on_kinematic_update(state);
}

void KinematicSystem::ui_widget() {
void CartesianKinematicSystem::ui_widget() {
if (state.effector_position.size() > 0) {
auto value = state.effector_position[0].position.x;
if (ImGui::SliderFloat("X Position(mm)", &value, X_MIN_POS, X_MAX_POS)) {
Expand Down Expand Up @@ -253,19 +256,20 @@ void KinematicSystem::ui_widget() {
}
}

#if ENABLED(DELTA)
#define A_AXIS 0
#define B_AXIS 1
#define C_AXIS 2
enum DeltaAxis {
A_AXIS_IDX,
B_AXIS_IDX,
C_AXIS_IDX
};

// Stolen from Marlin Firmware delta.cpp
void DeltaKinematicSystem::recalc_delta_settings() {
//constexpr abc_float_t trt = DELTA_RADIUS_TRIM_TOWER;
delta_tower[A_AXIS] = { std::cos(glm::radians(210.0 + delta_tower_angle_trim.x)) * (delta_radius + delta_radius_trim_tower.x), // front left tower
delta_tower[A_AXIS_IDX] = { std::cos(glm::radians(210.0 + delta_tower_angle_trim.x)) * (delta_radius + delta_radius_trim_tower.x), // front left tower
std::sin(glm::radians(210.0 + delta_tower_angle_trim.x)) * (delta_radius + delta_radius_trim_tower.x) };
delta_tower[B_AXIS] = { std::cos(glm::radians(330.0 + delta_tower_angle_trim.y)) * (delta_radius + delta_radius_trim_tower.y), // front right tower
delta_tower[B_AXIS_IDX] = { std::cos(glm::radians(330.0 + delta_tower_angle_trim.y)) * (delta_radius + delta_radius_trim_tower.y), // front right tower
std::sin(glm::radians(330.0 + delta_tower_angle_trim.y)) * (delta_radius + delta_radius_trim_tower.y) };
delta_tower[C_AXIS] = { std::cos(glm::radians( 90.0 + delta_tower_angle_trim.z)) * (delta_radius + delta_radius_trim_tower.z), // back middle tower
delta_tower[C_AXIS_IDX] = { std::cos(glm::radians( 90.0 + delta_tower_angle_trim.z)) * (delta_radius + delta_radius_trim_tower.z), // back middle tower
std::sin(glm::radians( 90.0 + delta_tower_angle_trim.z)) * (delta_radius + delta_radius_trim_tower.z) };
delta_diagonal_rod_2_tower = { std::pow(delta_diagonal_rod + delta_diagonal_rod_trim.x, 2),
std::pow(delta_diagonal_rod + delta_diagonal_rod_trim.y, 2),
Expand All @@ -275,8 +279,8 @@ void DeltaKinematicSystem::recalc_delta_settings() {
glm::vec3 DeltaKinematicSystem::forward_kinematics(const double z1, const double z2, const double z3) {

// Create a vector in old coordinates along x axis of new coordinate
const double p12[3] = { delta_tower[B_AXIS].x - delta_tower[A_AXIS].x,
delta_tower[B_AXIS].y - delta_tower[A_AXIS].y,
const double p12[3] = { delta_tower[B_AXIS_IDX].x - delta_tower[A_AXIS_IDX].x,
delta_tower[B_AXIS_IDX].y - delta_tower[A_AXIS_IDX].y,
z2 - z1 };

// Get the reciprocal of Magnitude of vector.
Expand All @@ -288,8 +292,8 @@ glm::vec3 DeltaKinematicSystem::forward_kinematics(const double z1, const double
p12[2] * inv_d };

// Get the vector from the origin of the new system to the third point.
const double p13[3] = { delta_tower[C_AXIS].x - delta_tower[A_AXIS].x,
delta_tower[C_AXIS].y - delta_tower[A_AXIS].y,
const double p13[3] = { delta_tower[C_AXIS_IDX].x - delta_tower[A_AXIS_IDX].x,
delta_tower[C_AXIS_IDX].y - delta_tower[A_AXIS_IDX].y,
z3 - z1 };

// Use the dot product to find the component of this vector on the X axis.
Expand Down Expand Up @@ -331,63 +335,78 @@ glm::vec3 DeltaKinematicSystem::forward_kinematics(const double z1, const double
// Start from the origin of the old coordinates and add vectors in the
// old coords that represent the Xnew, Ynew and Znew to find the point
// in the old system.
return glm::vec3{ delta_tower[A_AXIS].x + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew,
delta_tower[A_AXIS].y + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew,
return glm::vec3{ delta_tower[A_AXIS_IDX].x + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew,
delta_tower[A_AXIS_IDX].y + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew,
z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew
};
}

DeltaKinematicSystem::DeltaKinematicSystem(std::function<void(glm::vec4)> on_kinematic_update) : VirtualPrinter::Component("Delta Kinematic System"), on_kinematic_update(on_kinematic_update) {
DeltaKinematicSystem::DeltaKinematicSystem(std::function<void(kinematic_state)> on_kinematic_update) : KinematicSystem(on_kinematic_update) {

steppers.push_back(add_component<StepperDriver>("Stepper0", X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, [this](){ this->kinematic_update(); }));
steppers.push_back(add_component<StepperDriver>("Stepper1", Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, [this](){ this->kinematic_update(); }));
steppers.push_back(add_component<StepperDriver>("Stepper2", Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, [this](){ this->kinematic_update(); }));
steppers.push_back(add_component<StepperDriver>("Stepper3", E0_ENABLE_PIN, E0_DIR_PIN, E0_STEP_PIN, [this](){ this->kinematic_update(); }));
recalc_delta_settings();
#ifdef DELTA_HEIGHT
delta_height = DELTA_HEIGHT;
#endif
#ifdef DELTA_RADIUS
delta_radius = DELTA_RADIUS;
#endif
#ifdef DELTA_DIAGONAL_ROD
delta_diagonal_rod = DELTA_DIAGONAL_ROD;
#endif

collect_steppers();

// Add an offset as on deltas the linear rails are offset from the bed
origin.x = 207.124;//215.0 + DELTA_HEIGHT;
origin.y = 207.124;//215.0 + DELTA_HEIGHT;
origin.z = 207.124;//215.0 + DELTA_HEIGHT;
recalc_delta_settings();
// Add an offset as on deltas the linear rails minimum position are offset from the bed
hardware_offset.push_back({ 217.0807, 217.0807, 217.0807 });
}

void DeltaKinematicSystem::kinematic_update() {
stepper_position = glm::vec4{
std::static_pointer_cast<StepperDriver>(steppers[0])->steps() / steps_per_unit[0] * (((INVERT_X_DIR * 2) - 1) * -1.0),
std::static_pointer_cast<StepperDriver>(steppers[1])->steps() / steps_per_unit[1] * (((INVERT_Y_DIR * 2) - 1) * -1.0),
std::static_pointer_cast<StepperDriver>(steppers[2])->steps() / steps_per_unit[2] * (((INVERT_Z_DIR * 2) - 1) * -1.0),
std::static_pointer_cast<StepperDriver>(steppers[3])->steps() / steps_per_unit[3] * (((INVERT_E0_DIR * 2) - 1) * -1.0)
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),
};

// Add an offset to fudge the coordinate system onto the bed
effector_position = glm::vec4{ forward_kinematics(stepper_position.x + origin.x, stepper_position.y + origin.y, stepper_position.z + origin.z), stepper_position.a} + glm::vec4{X_BED_SIZE / 2.0, Y_BED_SIZE / 2.0, 0, 0};
on_kinematic_update(effector_position);
std::vector<double> extruder {};
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));
}

#ifdef SINGLENOZZLE
for (size_t i = 0; i < EXTRUDERS; ++i) {
auto cartesian_pos = forward_kinematics(hardware_offset[0].x + carriage.x, hardware_offset[0].y + carriage.y, hardware_offset[0].z + carriage.z) + glm::vec3{X_BED_SIZE / 2, Y_BED_SIZE / 2, 0.0};
state.effector_position[i] = {carriage, glm::vec4(cartesian_pos, extruder[i]), filament_color[i]};
}
#else
#error Implement Offset kinematics for multiple offset hotends on a delta carriage ...
#endif

state.position = state.effector_position[0].position;
on_kinematic_update(state);
}

void DeltaKinematicSystem::ui_widget() {
auto value = stepper_position.x + origin.x;
if (ImGui::SliderFloat("Stepper(A) Position (mm)", &value, -100, DELTA_HEIGHT + 100)) {
origin.x = value - stepper_position.x;
auto value = hardware_offset[0].x + state.effector_position[0].stepper_position.x;
if (ImGui::SliderFloat("Stepper(A) Position (mm)", &value, -100, delta_height + 100)) {
hardware_offset[0].x = value - state.effector_position[0].stepper_position.x;
kinematic_update();
}
value = stepper_position.y + origin.y;
if (ImGui::SliderFloat("Stepper(B) Position (mm)", &value, -100, DELTA_HEIGHT + 100)) {
origin.y = value - stepper_position.y;
value = hardware_offset[0].y + state.effector_position[0].stepper_position.y;
if (ImGui::SliderFloat("Stepper(B) Position (mm)", &value, -100, delta_height + 100)) {
hardware_offset[0].y = value - state.effector_position[0].stepper_position.y;
kinematic_update();
}
value = stepper_position.z + origin.z;
if (ImGui::SliderFloat("Stepper(C) Position (mm)", &value, -100, DELTA_HEIGHT + 100)) {
origin.z = value - stepper_position.z;
value = hardware_offset[0].z + state.effector_position[0].stepper_position.z;
if (ImGui::SliderFloat("Stepper(C) Position (mm)", &value, -100, delta_height + 100)) {
hardware_offset[0].z = value - state.effector_position[0].stepper_position.z;
kinematic_update();
}
ImGui::Text("Stepper Position:");
ImGui::Text("x: %f", stepper_position.x);
ImGui::Text("y: %f", stepper_position.y);
ImGui::Text("z: %f", stepper_position.z);
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", effector_position.x);
ImGui::Text("y: %f", effector_position.y);
ImGui::Text("z: %f", effector_position.z);
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
29 changes: 16 additions & 13 deletions src/MarlinSimulator/hardware/KinematicSystem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,29 +23,32 @@ struct kinematic_state {

class KinematicSystem : public VirtualPrinter::Component {
public:
KinematicSystem(std::function<void(kinematic_state)> on_kinematic_update);
~KinematicSystem() {}
KinematicSystem(std::function<void(kinematic_state)> on_kinematic_update) : VirtualPrinter::Component("Kinematic System"), on_kinematic_update(on_kinematic_update) {};

virtual void kinematic_update() = 0;
void collect_steppers();

void ui_widget();
void kinematic_update();
std::vector<glm::vec3> hardware_offset {};
std::vector<std::shared_ptr<VirtualPrinter::Component>> steppers;
kinematic_state state{};
std::function<void(kinematic_state)> on_kinematic_update;
};

class DeltaKinematicSystem : public VirtualPrinter::Component {
class CartesianKinematicSystem : public KinematicSystem {
public:
DeltaKinematicSystem(std::function<void(kinematic_state)> on_kinematic_update);
~DeltaKinematicSystem() {}
CartesianKinematicSystem(std::function<void(kinematic_state)> on_kinematic_update);
virtual void ui_widget() override;
virtual void kinematic_update() override;
};

void ui_widget();
void kinematic_update();
class DeltaKinematicSystem : public KinematicSystem {
public:
DeltaKinematicSystem(std::function<void(kinematic_state)> on_kinematic_update);
virtual void ui_widget() override;
virtual void kinematic_update() override;

std::vector<std::shared_ptr<VirtualPrinter::Component>> steppers;
kinematic_state state{};
std::function<void(kinematic_state)> on_kinematic_update;
double delta_radius = 140.0;
double delta_height = 200.0;
double delta_radius = 124.0;
double delta_diagonal_rod = 250.0;
glm::vec2 delta_tower[3]{};
glm::vec3 delta_tower_angle_trim{};
Expand Down
5 changes: 3 additions & 2 deletions src/MarlinSimulator/hardware/SDCard.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,17 +141,18 @@ void SDCard::onRequestedDataReceived(uint8_t token, uint8_t* _data, size_t count
memset(buf+1, 0xC0, 3);
setResponse(buf, 4);
break;
case CMD17: //read block
case CMD17: { //read block
buf[0] = R1_READY_STATE;
buf[1] = DATA_START_BLOCK;
if (true /*_type != SD_CARD_TYPE_SDHC*/) {
currentArg >>= 9;
}
fseek(fp, 512 * currentArg, SEEK_SET);
fread(buf + 2, 512, 1, fp);
[[maybe_unused]] auto result = fread(buf + 2, 512, 1, fp);
buf[512 + 2] = 0; //crc
setResponse(buf, 512 + 3);
break;
}
case CMD24: //write block
if (true /*_type != SD_CARD_TYPE_SDHC*/) {
currentArg >>= 9;
Expand Down
10 changes: 5 additions & 5 deletions src/MarlinSimulator/virtual_printer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,11 +53,11 @@ void VirtualPrinter::build() {

#if 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.stepper_position.x >= Z_MAX_POS; });
root->add_component<EndStop>("Endstop(Tower B Max)", Y_MAX_PIN, !Y_MAX_ENDSTOP_HIT_STATE, [kinematics](){ return kinematics->state.stepper_position.y >= Z_MAX_POS; });
root->add_component<EndStop>("Endstop(Tower C Max)", Z_MAX_PIN, !Z_MAX_ENDSTOP_HIT_STATE, [kinematics](){ return kinematics->state.stepper_position.z >= Z_MAX_POS; });
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; });
root->add_component<EndStop>("Endstop(Tower C Max)", Z_MAX_PIN, !Z_MAX_ENDSTOP_HIT_STATE, [kinematics](){ return kinematics->state.effector_position[0].stepper_position.z >= DELTA_HEIGHT; });
#else
auto kinematics = root->add_component<KinematicSystem>("Cartesian Kinematic System", on_kinematic_update);
auto kinematics = root->add_component<CartesianKinematicSystem>("Cartesian Kinematic System", on_kinematic_update);
root->add_component<EndStop>("Endstop(X Min)", X_MIN_PIN, !X_MIN_ENDSTOP_HIT_STATE, [kinematics](){ return kinematics->state.effector_position[0].position.x <= X_MIN_POS; });
#ifdef DUAL_X_CARRIAGE
root->add_component<EndStop>("Endstop(X2 Max)", X_MAX_PIN, !X_MAX_ENDSTOP_HIT_STATE, [kinematics](){ return kinematics->state.effector_position[1].position.x >= X2_MAX_POS; });
Expand All @@ -75,7 +75,7 @@ void VirtualPrinter::build() {
#else
Z_MIN_PROBE_PIN
#endif
, glm::vec3 NOZZLE_TO_PROBE_OFFSET, kinematics->effector_position, *print_bed);
, glm::vec3 NOZZLE_TO_PROBE_OFFSET, kinematics->state.effector_position[0].position, *print_bed);
#endif

#if HOTENDS
Expand Down
22 changes: 12 additions & 10 deletions src/MarlinSimulator/visualisation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,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->state.effector_position.size() == extrusion.size()) {
size_t i = 0;
for (auto state : kin->state.effector_position) {
Expand Down Expand Up @@ -336,21 +337,22 @@ void Visualisation::ui_viewport_callback(UiWindow* window) {
if (follow_mode != FOLLOW_NONE)
follow_offset = camera.position - glm::vec3(ex.position);
}
if (ImGui::IsKeyPressed(SDL_SCANCODE_F1)) {
for (auto& mesh : m_renderer.m_mesh) {
mesh->m_visible = false;
}
}
if (ImGui::IsKeyPressed(SDL_SCANCODE_F2)) {
for (auto& mesh : m_renderer.m_mesh) {
mesh->m_visible = true;
}
}
if (ImGui::GetIO().MouseWheel != 0 && viewport.hovered) {
camera.position += camera.speed * camera.direction * delta * ImGui::GetIO().MouseWheel;
}
}

if (ImGui::IsKeyPressed(SDL_SCANCODE_F1)) {
for (auto& mesh : m_renderer.m_mesh) {
mesh->m_visible = false;
}
}
if (ImGui::IsKeyPressed(SDL_SCANCODE_F2)) {
for (auto& mesh : m_renderer.m_mesh) {
mesh->m_visible = true;
}
}

bool last_mouse_captured = mouse_captured;
if (ImGui::IsMouseDown(0) && viewport.hovered) {
mouse_captured = true;
Expand Down

0 comments on commit afe7683

Please sign in to comment.