From 7ef6e45f688e392c64187f5b8910478b6cdf3742 Mon Sep 17 00:00:00 2001 From: Michael Moon Date: Thu, 20 Feb 2014 09:25:46 +1100 Subject: [PATCH] Homer: new approach to homing. Unfinished implementation. --- src/modules/tools/homer/Homer.cpp | 438 ++++++++++-------------------- src/modules/tools/homer/Homer.h | 13 +- 2 files changed, 152 insertions(+), 299 deletions(-) rewrite src/modules/tools/homer/Homer.cpp (82%) diff --git a/src/modules/tools/homer/Homer.cpp b/src/modules/tools/homer/Homer.cpp dissimilarity index 82% index ac341aeb..8c072da9 100644 --- a/src/modules/tools/homer/Homer.cpp +++ b/src/modules/tools/homer/Homer.cpp @@ -1,296 +1,142 @@ -/* - This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). - Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - You should have received a copy of the GNU General Public License along with Smoothie. If not, see . -*/ - -#include "Homer.h" -#include "Kernel.h" -#include "Robot.h" -#include "StepperMotor.h" -#include "Planner.h" -#include "Conveyor.h" -#include "Endstop.h" -#include "Gcode.h" -#include "arm_solutions/BaseSolution.h" - -#include - -Homer::Homer() -{ -} - -void Homer::on_module_loaded() -{ - register_for_event(ON_GCODE_RECEIVED); -} - -void Homer::home_actuators(Gcode* gcode) -{ - uint8_t actuators[3] = {NO_HOME, NO_HOME, NO_HOME}; - - bool home_all = true; - for (int i = 0; i < 3; i++) - { - if (gcode->has_letter('A' + i) || gcode->has_letter('X' + i)) - home_all = false; - } - - // work out which actuators are selected for homing - for (int i = 0; i < 3; i++) - { - if (home_all || gcode->has_letter('A' + i)) - { - Actuator* act = THEKERNEL->robot->actuators[i]; - - if (act->min_stop && act->max_stop) - { - if (gcode->get_value('A' + i) > 0.0F) - actuators[i] = HOME_TO_MAX; - else - actuators[i] = HOME_TO_MIN; - } - else if (act->min_stop) - actuators[i] = HOME_TO_MIN; - else if (act->max_stop) - actuators[i] = HOME_TO_MAX; - else - actuators[i] = NO_HOME; - } - } - - if (THEKERNEL->robot->arm_solution->confirm_homing_set(actuators) == false) - { - // arm solution says illegal combination - gcode->stream->printf("Error: Illegal actuator set while homing!\n"); - return; - } - - if ((actuators[0] == 0) && (actuators[1] == 0) && (actuators[2] == 0)) - return; - - float distance = 0.0F; - float dest_vector[3] = {0.0F, 0.0F, 0.0F}; - - for (int i = 0; i < 3; i++) - { - if (actuators[i]) - { - Actuator* act = THEKERNEL->robot->actuators[i]; - - // work out actuator travel from soft limits - dest_vector[i] = act->soft_max - act->soft_min; - - // if soft limits are NAN, use a default of 500mm - if (isnan(dest_vector[i])) - dest_vector[i] = 500.0F; - - // find actuator that moves greatest distance - if (dest_vector[i] > distance) - distance = dest_vector[i]; - - // move in negative direction if homing to min - if (actuators[i] == HOME_TO_MIN) - dest_vector[i] *= -1.0F; - - // add current position, so we can work out position where endstop triggered - dest_vector[i] += act->last_milestone; - } - } - - if (distance > 0.0F) - { - float unit_vec[3] = {0.0F, 0.0F, 0.0F}; - bool check[3]; - - THEKERNEL->planner->append_block(dest_vector, 10.0F, distance, unit_vec); - - // set all active endstops to be checked - for (int i = 0; i < 3; i++) - { - if (THEKERNEL->robot->actuators[i]->active_stop) - { - check[i] = THEKERNEL->robot->actuators[i]->active_stop->check; - THEKERNEL->robot->actuators[i]->active_stop->check = true; - } - } - - // wait for move to complete - THEKERNEL->conveyor->wait_for_empty_queue(); - - // update actuator positions - for (int i = 0; i < 3; i++) - { - if (THEKERNEL->robot->actuators[i]->active_stop) - { - THEKERNEL->robot->actuators[i]->active_stop->check = check[i]; - // TODO: cache or report endstop trigger position - THEKERNEL->robot->actuators[i]->change_last_milestone(THEKERNEL->robot->actuators[i]->active_stop->position); - } - } - } - - // reuse dest_vector as current actuator position - for (int i = 0; i < 3; i++) - dest_vector[i] = THEKERNEL->robot->actuators[i]->last_milestone; - - float cartesian[3]; - - // perform forward kinematics to find cartesian position - THEKERNEL->robot->arm_solution->actuator_to_cartesian(dest_vector, cartesian); - - // update axis positions - for (int i = 0; i < 3; i++) - THEKERNEL->robot->axes[i].change_last_milestone(cartesian[i]); -} - -void Homer::home_axes(Gcode* gcode) -{ - uint8_t axes[3] = {NO_HOME, NO_HOME, NO_HOME}; - - // work out which axes are selected for homing - bool home_all = true; - for (int i = 0; i < 3; i++) - { - if (gcode->has_letter('A' + i) || gcode->has_letter('X' + i)) - home_all = false; - } - - for (int i = 0; i < 3; i++) - { - if (home_all || gcode->has_letter('X' + i)) - { - Actuator* act = &THEKERNEL->robot->axes[i]; - - if (act->min_stop && act->max_stop) - { - if (gcode->get_value('X' + i) > 0.0F) - axes[i] = HOME_TO_MAX; - else - axes[i] = HOME_TO_MIN; - } - else if (act->min_stop) - axes[i] = HOME_TO_MIN; - else if (act->max_stop) - axes[i] = HOME_TO_MAX; - else - axes[i] = NO_HOME; - } - } - - if ((axes[0] == 0) && (axes[1] == 0) && (axes[2] == 0)) - return; - - while ((axes[0] != 0) || (axes[1] != 0) || (axes[2] != 0)) - { - float distance = 0.0F; - float dest_vector[3] = {0.0F, 0.0F, 0.0F}; - - for (int i = 0; i < 3; i++) - { - if (axes[i]) - { - Actuator* act = &THEKERNEL->robot->axes[i]; - - // work out actuator travel from soft limits - dest_vector[i] = act->soft_max - act->soft_min; - - // if soft limits are NAN, use a default of 500mm - if (isnan(dest_vector[i])) - dest_vector[i] = 500.0F; - - // find actuator that moves greatest distance - if (dest_vector[i] > distance) - distance = dest_vector[i]; - - // move in negative direction if homing to min - if (axes[i] == HOME_TO_MIN) - dest_vector[i] *= -1.0F; - - // add current position, so we can work out position where endstop triggered - dest_vector[i] += act->last_milestone; - } - } - - if (distance > 0.0F) - { - float unit_vec[3] = {0.0F, 0.0F, 0.0F}; - float act_vector[3]; - - THEKERNEL->robot->arm_solution->cartesian_to_actuator(dest_vector, act_vector); - - THEKERNEL->planner->append_block(act_vector, 10.0F, distance, unit_vec); - - // set up endstops - for (int i = 0; i < 3; i++) - { - Actuator* axis = &THEKERNEL->robot->axes[i]; - if (axes[i] == HOME_TO_MAX) - axis->active_stop = axis->max_stop; - else if (axes[i] == HOME_TO_MIN) - axis->active_stop = axis->min_stop; - } - - // wait for move to complete - while (THEKERNEL->conveyor->queue.is_empty() == false) - { - int triggered = -1; - for (int i = 0; i < 3; i++) - { - Actuator* axis = &THEKERNEL->robot->axes[i]; - if ((axes[i] != NO_HOME) && axis->active_stop && axis->active_stop->asserted()) - { - // endstop triggered! - triggered = i; - break; - } - } - if (triggered >= 0) - { - // stop all actuators - // TODO: decelerate instead of stopping dead - for (int i = 0; i < 3; i++) - THEKERNEL->robot->actuators[i]->move(0, 0); - - axes[triggered] = NO_HOME; - - Actuator* axis = &THEKERNEL->robot->axes[triggered]; - - THEKERNEL->conveyor->wait_for_empty_queue(); - - axis->change_last_milestone(axis->active_stop->position); - axis->active_stop = NULL; - } - } - } - } - - // update positions - float cartesian[3], actuator[3]; - - for (int i = 0; i < 3; i++) - cartesian[i] = THEKERNEL->robot->axes[i].last_milestone; - - THEKERNEL->robot->arm_solution->cartesian_to_actuator(cartesian, actuator); - - for (int i = 0; i < 3; i++) - THEKERNEL->robot->actuators[i]->change_last_milestone(actuator[i]); -} - -// Start homing sequences by response to GCode commands -void Homer::on_gcode_received(void *argument) -{ - Gcode *gcode = static_cast(argument); - if (gcode->has_g && gcode->g == 28) - { - gcode->mark_as_taken(); - // G28 is received, we have homing to do - - // First wait for the queue to be empty - THEKERNEL->conveyor->wait_for_empty_queue(); - - home_actuators(gcode); - home_axes(gcode); - } -} +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. + Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +#include "Homer.h" +#include "Kernel.h" +#include "Robot.h" +#include "StepperMotor.h" +#include "Planner.h" +#include "Conveyor.h" +#include "Endstop.h" +#include "Gcode.h" +#include "CartesianAxis.h" +#include "arm_solutions/BaseSolution.h" + +#include + +#define homing_order_checksum CHECKSUM("homing_order") + +Homer::Homer() +{ +} + +void Homer::on_module_loaded() +{ + register_for_event(ON_GCODE_RECEIVED); + register_for_event(ON_CONFIG_RELOAD); + + on_config_reload(this); +} + +void Homer::on_config_reload(void*) +{ + homing_order = THEKERNEL->config->value(homing_order_checksum)->by_default("ABC,XY,Z")->as_string(); +} + +// Start homing sequences by response to GCode commands +void Homer::on_gcode_received(void *argument) +{ + Gcode *gcode = static_cast(argument); + + if (gcode->has_g && gcode->g == 28) + { + gcode->mark_as_taken(); + // G28 is received, we have homing to do + + // wait for the queue to be empty + THEKERNEL->conveyor->wait_for_empty_queue(); + + home_set(assemble_set_from_gcode(gcode)); + } +} + +string Homer::assemble_set_from_gcode(Gcode* gcode) +{ + string set = homing_order; + bool config_order = true; + + for (int i = 0; i < 3; i++) + { + if (gcode->has_letter('A' + i)) + { + if (config_order) + set.clear(); + config_order = false; + + set += 'A' + i; + + // if user sends G28 A1, home to max + if (gcode->get_value('A' + i) > 0.0F) + set += '+'; + } + if (gcode->has_letter('X' + i)) + { + if (config_order) + set.clear(); + config_order = false; + + set += 'X' + i; + + // if user sends G28 X1, home to max + if (gcode->get_value('X' + i) > 0.0F) + set += '+'; + } + } + + // ensure last item gets homed + set += ','; + + return set; +} + +void Homer::home_set(string set) +{ + struct homing_info + { + Actuator* actuator; + + struct { + bool home_to_max :1; + bool is_axis :1; + }; + }; + + std::vector actuators; + actuators.reserve(3); + + for (auto i = set.begin(); i != set.end(); i++) + { + if (*i >= 'A' && *i <= 'C') + { + actuators.resize(actuators.size() + 1); + actuators.back().actuator = THEKERNEL->robot->actuators[*i - 'A']; + actuators.back().is_axis = false; + } + else if (*i >= 'X' && *i <= 'Z') + { + actuators.resize(actuators.size() + 1); + actuators.back().actuator = &THEKERNEL->robot->axes[*i - 'X']; + actuators.back().is_axis = true; + } + else if (*i == '+') + actuators.back().home_to_max = true; + else if (*i == '-') + actuators.back().home_to_max = false; + else if (*i == ',') + { + // TODO: home this set + + THEKERNEL->serial->printf("Homing Set:\n"); + for (auto j = actuators.begin(); j != actuators.end(); j++) + { + THEKERNEL->serial->printf("\t%p: %c to %s\n", j->actuator, j->actuator->designator, j->home_to_max?"MAX":"MIN"); + } + + actuators.clear(); + } + } +} diff --git a/src/modules/tools/homer/Homer.h b/src/modules/tools/homer/Homer.h index cb7160ad..52c3a5ed 100644 --- a/src/modules/tools/homer/Homer.h +++ b/src/modules/tools/homer/Homer.h @@ -10,22 +10,29 @@ #include "libs/Module.h" +#include + #define HOME_TO_MAX 2 #define HOME_TO_MIN 1 #define NO_HOME 0 class Gcode; +using std::string; + class Homer : public Module { public: Homer(); void on_module_loaded(); - void on_gcode_received(void* argument); + void on_gcode_received(void*); + void on_config_reload( void*); private: - void home_actuators(Gcode* gcode); - void home_axes(Gcode* gcode); + string assemble_set_from_gcode(Gcode*); + void home_set(string); + + string homing_order; }; #endif /* _HOMER_H */ -- 2.20.1