#define THEKERNEL Kernel::instance
#define THECONVEYOR THEKERNEL->conveyor
+#define THEROBOT THEKERNEL->robot
#include "Module.h"
#include <array>
++current_block->tick_info[m].step_count;
// step the motor
- motor[m]->step();
+ bool ismoving= motor[m]->step(); // returns false if the moving flag was set to false externally (probes, endstops etc)
// we stepped so schedule an unstep
unstep.set(m);
- if(current_block->tick_info[m].step_count == current_block->tick_info[m].steps_to_move) {
+ if(!ismoving || current_block->tick_info[m].step_count == current_block->tick_info[m].steps_to_move) {
// done
current_block->tick_info[m].steps_to_move = 0;
motor[m]->moving= false; // let motor know it is no longer moving
~StepperMotor();
// called from step ticker ISR
- inline void step() { step_pin.set(1); current_position_steps += (direction?-1:1); }
+ inline bool step() { step_pin.set(1); current_position_steps += (direction?-1:1); return moving; }
// called from unstep ISR
inline void unstep() { step_pin.set(0); }
// called from step ticker ISR
inline void enable(bool state) { en_pin.set(!state); };
inline bool is_enabled() const { return !en_pin.get(); };
inline bool is_moving() const { return moving; };
+ void stop_moving() { moving= false; }
void manual_step(bool dir);
}
}
// makes it handle the parameters as a machine position
- THEKERNEL->robot->next_command_is_MCS= true;
+ THEROBOT->next_command_is_MCS= true;
}
bool Conveyor::is_idle() const
{
if(queue.is_empty()) {
- for(auto &a : THEKERNEL->robot->actuators) {
+ for(auto &a : THEROBOT->actuators) {
if(a->is_moving()) return false;
}
return true;
// Append a block to the queue, compute it's speed factors
-void Planner::append_block( ActuatorCoordinates &actuator_pos, float rate_mm_s, float distance, float unit_vec[] )
+void Planner::append_block( ActuatorCoordinates &actuator_pos, float rate_mm_s, float distance, float *unit_vec)
{
float acceleration, junction_deviation;
// Direction bits
- for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
- int steps = THEKERNEL->robot->actuators[i]->steps_to_target(actuator_pos[i]);
+ for (size_t i = 0; i < THEROBOT->actuators.size(); i++) {
+ int steps = THEROBOT->actuators[i]->steps_to_target(actuator_pos[i]);
block->direction_bits[i] = (steps < 0) ? 1 : 0;
// Update current position
- THEKERNEL->robot->actuators[i]->last_milestone_steps += steps;
- THEKERNEL->robot->actuators[i]->last_milestone_mm = actuator_pos[i];
+ THEROBOT->actuators[i]->last_milestone_steps += steps;
+ THEROBOT->actuators[i]->last_milestone_mm = actuator_pos[i];
block->steps[i] = labs(steps);
}
block->acceleration = acceleration; // save in block
+ // if it is a SOLO move from extruder, zprobe or endstops we do not use junction deviation
+ if(unit_vec == nullptr) {
+ junction_deviation= 0.0F;
+ }
+
// Max number of steps, for all axes
uint32_t steps_event_count = 0;
- for (size_t s = 0; s < THEKERNEL->robot->actuators.size(); s++) {
+ for (size_t s = 0; s < THEROBOT->actuators.size(); s++) {
steps_event_count = std::max(steps_event_count, block->steps[s]);
}
block->steps_event_count = steps_event_count;
block->recalculate_flag = true;
// Update previous path unit_vector and nominal speed
- memcpy(this->previous_unit_vec, unit_vec, sizeof(previous_unit_vec)); // previous_unit_vec[] = unit_vec[]
+ if(unit_vec != nullptr) {
+ memcpy(this->previous_unit_vec, unit_vec, sizeof(previous_unit_vec)); // previous_unit_vec[] = unit_vec[]
+ }else{
+ clear_vector_float(this->previous_unit_vec);
+ }
// Math-heavy re-computing of the whole queue to take the new
this->recalculate();
{
public:
Planner();
- void append_block(ActuatorCoordinates &target, float rate_mm_s, float distance, float unit_vec[] );
float max_allowable_speed( float acceleration, float target_velocity, float distance);
- void recalculate();
float get_acceleration() const { return acceleration; }
float get_z_acceleration() const { return z_acceleration > 0.0F ? z_acceleration : acceleration; }
friend class Robot; // for acceleration, junction deviation, minimum_planner_speed
private:
+ void append_block(ActuatorCoordinates &target, float rate_mm_s, float distance, float unit_vec[] );
+ void recalculate();
void config_load();
float previous_unit_vec[3];
float acceleration; // Setting
// Convert target (in machine coordinates) from millimeters to steps, and append this to the planner
// target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
// all transforms and is what we actually convert to actuator positions
-bool Robot::append_milestone(Gcode * gcode, const float target[], float rate_mm_s)
+bool Robot::append_milestone(Gcode *gcode, const float target[], float rate_mm_s)
{
float deltas[3];
float unit_vec[3];
return true;
}
+// Used to plan a single move used by things like endstops when homing, zprobe, extruder retracts etc.
+bool Robot::solo_move(const float *delta, float rate_mm_s)
+{
+ if(THEKERNEL->is_halted()) return false;
+
+ // catch negative or zero feed rates and return the same error as GRBL does
+ if(rate_mm_s <= 0.0F) {
+ return false;
+ }
+
+ // Compute how long this move moves, so we can attach it to the block for later use
+ float millimeters_of_travel = sqrtf( powf( delta[X_AXIS], 2 ) + powf( delta[Y_AXIS], 2 ) + powf( delta[Z_AXIS], 2 ) );
+
+ // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
+ // as the last milestone won't be updated we do not actually lose any moves as they will be accounted for in the next move
+ if(millimeters_of_travel < 0.00001F) return false;
+
+ // this is the new machine position
+ for (int axis = X_AXIS; axis <= Z_AXIS; axis++) {
+ this->last_machine_position[axis] += delta[axis];
+ }
+
+ // Do not move faster than the configured cartesian limits
+ for (int axis = X_AXIS; axis <= Z_AXIS; axis++) {
+ if ( max_speeds[axis] > 0 ) {
+ float axis_speed = fabsf(delta[axis] / millimeters_of_travel * rate_mm_s);
+
+ if (axis_speed > max_speeds[axis])
+ rate_mm_s *= ( max_speeds[axis] / axis_speed );
+ }
+ }
+
+ // find actuator position given the machine position, use actual adjusted target
+ ActuatorCoordinates actuator_pos;
+ arm_solution->cartesian_to_actuator( this->last_machine_position, actuator_pos );
+
+ float isecs = rate_mm_s / millimeters_of_travel;
+ // check per-actuator speed limits
+ for (size_t actuator = 0; actuator < actuators.size(); actuator++) {
+ float actuator_rate = fabsf(actuator_pos[actuator] - actuators[actuator]->last_milestone_mm) * isecs;
+ if (actuator_rate > actuators[actuator]->get_max_rate()) {
+ rate_mm_s *= (actuators[actuator]->get_max_rate() / actuator_rate);
+ isecs = rate_mm_s / millimeters_of_travel;
+ }
+ }
+
+ // Append the block to the planner
+ THEKERNEL->planner->append_block(actuator_pos, rate_mm_s, millimeters_of_travel, nullptr);
+
+ return true;
+}
+
// Append a move to the queue ( cutting it into segments if needed )
bool Robot::append_line(Gcode *gcode, const float target[], float rate_mm_s )
{
std::vector<wcs_t> get_wcs_state() const;
std::tuple<float, float, float, uint8_t> get_last_probe_position() const { return last_probe_position; }
void set_last_probe_position(std::tuple<float, float, float, uint8_t> p) { last_probe_position = p; }
+ bool solo_move(const float target[], float rate_mm_s);
BaseSolution* arm_solution; // Selected Arm solution ( millimeters to step calculation )
THEKERNEL->conveyor->wait_for_empty_queue();
// get actual position from robot
float pos[3];
- THEKERNEL->robot->get_axis_position(pos);
+ THEROBOT->get_axis_position(pos);
// convert to WCS
- Robot::wcs_t wpos= THEKERNEL->robot->mcs2wcs(pos);
+ Robot::wcs_t wpos= THEROBOT->mcs2wcs(pos);
// backup Z position as Initial-Z value
this->initial_z = std::get<X_AXIS>(wpos); // must use the work coordinate position
// set retract type
// in cycle
else if (this->cycle_started) {
// relative mode not supported for now...
- if (THEKERNEL->robot->absolute_mode == false) {
+ if (THEROBOT->absolute_mode == false) {
gcode->stream->printf("Drillingcycles: relative mode not supported.\r\n");
gcode->stream->printf("Drillingcycles: skip hole...\r\n");
// exit
#include "libs/StepperMotor.h"
#include "wait_api.h" // mbed.h lib
#include "Robot.h"
-#include "Stepper.h"
#include "Config.h"
#include "SlowTicker.h"
#include "Planner.h"
#define homing_order_checksum CHECKSUM("homing_order")
#define move_to_origin_checksum CHECKSUM("move_to_origin_after_home")
-#define STEPPER THEKERNEL->robot->actuators
+#define STEPPER THEROBOT->actuators
#define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm())
{
this->status = NOT_HOMING;
home_offset[0] = home_offset[1] = home_offset[2] = 0.0F;
+ debounce.fill(0);
}
void Endstops::on_module_loaded()
register_for_event(ON_GET_PUBLIC_DATA);
register_for_event(ON_SET_PUBLIC_DATA);
- THEKERNEL->step_ticker->register_acceleration_tick_handler([this]() {acceleration_tick(); });
-
// Settings
this->load_config();
+
+ THEKERNEL->slow_ticker->attach(1000, this, &Endstops::read_endstops);
}
// Get config
this->homing_position[1] = this->home_direction[1] ? THEKERNEL->config->value(beta_min_checksum )->by_default(0)->as_number() : THEKERNEL->config->value(beta_max_checksum )->by_default(200)->as_number();
this->homing_position[2] = this->home_direction[2] ? THEKERNEL->config->value(gamma_min_checksum)->by_default(0)->as_number() : THEKERNEL->config->value(gamma_max_checksum)->by_default(200)->as_number();
+ // used to set maximum movement on homing
+ this->alpha_max= THEKERNEL->config->value(alpha_max_checksum)->by_default(500)->as_number();
+ this->beta_max= THEKERNEL->config->value(beta_max_checksum)->by_default(500)->as_number();
+ this->gamma_max= THEKERNEL->config->value(gamma_max_checksum)->by_default(500)->as_number();
+
this->is_corexy = THEKERNEL->config->value(corexy_homing_checksum)->by_default(false)->as_bool();
this->is_delta = THEKERNEL->config->value(delta_homing_checksum)->by_default(false)->as_bool();
this->is_rdelta = THEKERNEL->config->value(rdelta_homing_checksum)->by_default(false)->as_bool();
this->limit_enable[Y_AXIS] = THEKERNEL->config->value(beta_limit_enable_checksum)->by_default(false)->as_bool();
this->limit_enable[Z_AXIS] = THEKERNEL->config->value(gamma_limit_enable_checksum)->by_default(false)->as_bool();
- // set to true by default for deltas duwe to trim, false on cartesians
+ // set to true by default for deltas due to trim, false on cartesians
this->move_to_origin_after_home = THEKERNEL->config->value(move_to_origin_checksum)->by_default(is_delta)->as_bool();
if(this->limit_enable[X_AXIS] || this->limit_enable[Y_AXIS] || this->limit_enable[Z_AXIS]) {
// if limit switches are enabled, then we must move off of the endstop otherwise we won't be able to move
// checks if triggered and only backs off if triggered
-void Endstops::back_off_home(char axes_to_move)
+void Endstops::back_off_home()
{
std::vector<std::pair<char, float>> params;
this->status = BACK_OFF_HOME;
} else {
// cartesians, concatenate all the moves we need to do into one gcode
for( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if( ((axes_to_move >> c ) & 1) == 0) continue; // only for axes we asked to move
+ if(!axis_to_home[c]) continue; // only for axes we asked to move
// if not triggered no need to move off
if(this->limit_enable[c] && debounced_get(c + (this->home_direction[c] ? 0 : 3)) ) {
char gcode_buf[64];
append_parameters(gcode_buf, params, sizeof(gcode_buf));
Gcode gc(gcode_buf, &(StreamOutput::NullStream));
- THEKERNEL->robot->push_state();
- THEKERNEL->robot->absolute_mode = false; // needs to be relative mode
- THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
+ THEROBOT->push_state();
+ THEROBOT->absolute_mode = false; // needs to be relative mode
+ THEROBOT->on_gcode_received(&gc); // send to robot directly
// Wait for above to finish
THEKERNEL->conveyor->wait_for_empty_queue();
- THEKERNEL->robot->pop_state();
+ THEROBOT->pop_state();
}
this->status = NOT_HOMING;
}
// If enabled will move the head to 0,0 after homing, but only if X and Y were set to home
-void Endstops::move_to_origin(char axes_to_move)
+void Endstops::move_to_origin()
{
- if( (axes_to_move & 0x03) != 3 ) return; // ignore if X and Y not homing
+ if(!(axis_to_home[X_AXIS] && axis_to_home[Y_AXIS])) return; // ignore if X and Y not homing
// Do we need to check if we are already at 0,0? probably not as the G0 will not do anything if we are
- // float pos[3]; THEKERNEL->robot->get_axis_position(pos); if(pos[0] == 0 && pos[1] == 0) return;
+ // float pos[3]; THEROBOT->get_axis_position(pos); if(pos[0] == 0 && pos[1] == 0) return;
this->status = MOVE_TO_ORIGIN;
// Move to center using a regular move, use slower of X and Y fast rate
float rate = std::min(this->fast_rates[0], this->fast_rates[1]) * 60.0F;
char buf[32];
snprintf(buf, sizeof(buf), "G53 G0 X0 Y0 F%1.4f", rate); // must use machine coordinates in case G92 or WCS is in effect
- THEKERNEL->robot->push_state();
+ THEROBOT->push_state();
struct SerialMessage message;
message.message = buf;
message.stream = &(StreamOutput::NullStream);
THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); // as it is a multi G code command
// Wait for above to finish
THEKERNEL->conveyor->wait_for_empty_queue();
- THEKERNEL->robot->pop_state();
+ THEROBOT->pop_state();
this->status = NOT_HOMING;
}
-bool Endstops::wait_for_homed(char axes_to_move)
+// Called every millisecond in an ISR
+uint32_t Endstops::read_endstops(uint32_t dummy)
{
- bool running = true;
- unsigned int debounce[3] = {0, 0, 0};
- while (running) {
- running = false;
- THEKERNEL->call_event(ON_IDLE);
-
- // check if on_halt (eg kill)
- if(THEKERNEL->is_halted()) return false;
+ if(this->status >= NOT_HOMING) return 0; // not doing anything we need to monitor for
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( ( axes_to_move >> c ) & 1 ) {
- if ( this->pins[c + (this->home_direction[c] ? 0 : 3)].get() ) {
- if ( debounce[c] < debounce_count ) {
- debounce[c]++;
- running = true;
- } else if ( STEPPER[c]->is_moving() ) {
- STEPPER[c]->move(0, 0);
- axes_to_move &= ~(1 << c); // no need to check it again
+ if(!is_corexy) {
+ // check each axis
+ for ( int m = X_AXIS; m <= Z_AXIS; m++ ) {
+ if(STEPPER[m]->is_moving()) {
+ // if it is moving then we check the associated endstop, and debounce it
+ if(this->pins[m + (this->home_direction[m] ? 0 : 3)].get()) {
+ if(debounce[m] < debounce_count) {
+ debounce[m]++;
+ } else {
+ // we signal the motor to stop, which will preempt any moves on that axis
+ STEPPER[m]->stop_moving();
}
+
} else {
// The endstop was not hit yet
- running = true;
- debounce[c] = 0;
+ debounce[m] = 0;
}
}
}
- }
- return true;
-}
-
-void Endstops::do_homing_cartesian(char axes_to_move)
-{
- // check if on_halt (eg kill)
- if(THEKERNEL->is_halted()) return;
- // this homing works for cartesian and delta printers
- // Start moving the axes to the origin
- this->status = MOVING_TO_ENDSTOP_FAST;
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( ( axes_to_move >> c) & 1 ) {
- this->feed_rate[c] = this->fast_rates[c];
- STEPPER[c]->move(this->home_direction[c], 10000000, 0);
- }
- }
-
- // Wait for all axes to have homed
- if(!this->wait_for_homed(axes_to_move)) return;
-
- // Move back a small distance
- this->status = MOVING_BACK;
- bool inverted_dir;
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( ( axes_to_move >> c ) & 1 ) {
- inverted_dir = !this->home_direction[c];
- this->feed_rate[c] = this->slow_rates[c];
- STEPPER[c]->move(inverted_dir, this->retract_mm[c]*STEPS_PER_MM(c), 0);
- }
- }
+ } else {
+ // corexy is different as the actuators are not directly related to the XY axis
+ // so we check the axis that is currently homing then stop all motors
+ for ( int m = X_AXIS; m <= Z_AXIS; m++ ) {
+ if(axis_to_home[m]) {
+ if(this->pins[m + (this->home_direction[m] ? 0 : 3)].get()) {
+ if(debounce[m] < debounce_count) {
+ debounce[m]++;
+ } else {
+ // we signal all the motors to stop, as on corexy X and Y motors will move for X and Y axis homing and we only hom eone axis at a time
+ STEPPER[X_AXIS]->stop_moving();
+ STEPPER[Y_AXIS]->stop_moving();
+ STEPPER[Z_AXIS]->stop_moving();
+ }
- // Wait for moves to be done
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( ( axes_to_move >> c ) & 1 ) {
- while ( STEPPER[c]->is_moving() ) {
- THEKERNEL->call_event(ON_IDLE);
- if(THEKERNEL->is_halted()) return;
+ } else {
+ // The endstop was not hit yet
+ debounce[m] = 0;
+ }
}
}
}
- // Start moving the axes to the origin slowly
- this->status = MOVING_TO_ENDSTOP_SLOW;
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( ( axes_to_move >> c ) & 1 ) {
- this->feed_rate[c] = this->slow_rates[c];
- STEPPER[c]->move(this->home_direction[c], 10000000, 0);
- }
- }
-
- // Wait for all axes to have homed
- if(!this->wait_for_homed(axes_to_move)) return;
-}
-
-bool Endstops::wait_for_homed_corexy(int axis)
-{
- bool running = true;
- unsigned int debounce[3] = {0, 0, 0};
- while (running) {
- running = false;
- THEKERNEL->call_event(ON_IDLE);
-
- // check if on_halt (eg kill)
- if(THEKERNEL->is_halted()) return false;
-
- if ( this->pins[axis + (this->home_direction[axis] ? 0 : 3)].get() ) {
- if ( debounce[axis] < debounce_count ) {
- debounce[axis] ++;
- running = true;
- } else {
- // turn both off if running
- if (STEPPER[X_AXIS]->is_moving()) STEPPER[X_AXIS]->move(0, 0);
- if (STEPPER[Y_AXIS]->is_moving()) STEPPER[Y_AXIS]->move(0, 0);
- }
- } else {
- // The endstop was not hit yet
- running = true;
- debounce[axis] = 0;
- }
- }
- return true;
+ return 0;
}
-void Endstops::corexy_home(int home_axis, bool dirx, bool diry, float fast_rate, float slow_rate, unsigned int retract_steps)
+void Endstops::home()
{
- // check if on_halt (eg kill)
- if(THEKERNEL->is_halted()) return;
+ // reset debounce counts
+ debounce.fill(0);
+ // Start moving the axes to the origin
this->status = MOVING_TO_ENDSTOP_FAST;
- this->feed_rate[X_AXIS] = fast_rate;
- STEPPER[X_AXIS]->move(dirx, 10000000, 0);
- this->feed_rate[Y_AXIS] = fast_rate;
- STEPPER[Y_AXIS]->move(diry, 10000000, 0);
- // wait for primary axis
- if(!this->wait_for_homed_corexy(home_axis)) return;
-
- // Move back a small distance
+ if(axis_to_home[X_AXIS] && axis_to_home[Y_AXIS]) {
+ // Home XY first so as not to slow them down by homing Z at the same time
+ float delta[3] {alpha_max, beta_max, 0};
+ if(!this->home_direction[X_AXIS]) delta[X_AXIS]= -delta[X_AXIS];
+ if(!this->home_direction[Y_AXIS]) delta[Y_AXIS]= -delta[Y_AXIS];
+ float feed_rate = std::min(fast_rates[X_AXIS], fast_rates[Y_AXIS]);
+ THEROBOT->solo_move(delta, feed_rate);
+
+ // Wait for XY to have homed
+ THECONVEYOR->wait_for_empty_queue();
+
+ } else if(axis_to_home[X_AXIS]) {
+ // now home X only
+ float delta[3] {alpha_max, 0, 0};
+ if(!this->home_direction[X_AXIS]) delta[X_AXIS]= -delta[X_AXIS];
+ THEROBOT->solo_move(delta, fast_rates[X_AXIS]);
+ // wait for X
+ THECONVEYOR->wait_for_empty_queue();
+
+ } else if(axis_to_home[Y_AXIS]) {
+ // now home Y only
+ float delta[3] {0, beta_max, 0};
+ if(!this->home_direction[Y_AXIS]) delta[Y_AXIS]= -delta[Y_AXIS];
+ THEROBOT->solo_move(delta, fast_rates[Y_AXIS]);
+ // wait for Y
+ THECONVEYOR->wait_for_empty_queue();
+ }
+
+ if(axis_to_home[Z_AXIS]) {
+ // now home z
+ float delta[3] {0, 0, gamma_max};
+ if(!this->home_direction[Z_AXIS]) delta[Z_AXIS]= -delta[Z_AXIS];
+ THEROBOT->solo_move(delta, fast_rates[Z_AXIS]);
+ // wait for Z
+ THECONVEYOR->wait_for_empty_queue();
+ }
+
+ float delta[3]{0,0,0};
+ // use minimum feed rate of all three axes
+ float feed_rate= std::min(slow_rates[X_AXIS], std::min(slow_rates[Y_AXIS], slow_rates[Z_AXIS]));
+ // Move back a small distance for all homing axis
this->status = MOVING_BACK;
- this->feed_rate[X_AXIS] = slow_rate;
- STEPPER[X_AXIS]->move(!dirx, retract_steps, 0);
- this->feed_rate[Y_AXIS] = slow_rate;
- STEPPER[Y_AXIS]->move(!diry, retract_steps, 0);
-
- // wait until done
- while ( STEPPER[X_AXIS]->is_moving() || STEPPER[Y_AXIS]->is_moving()) {
- THEKERNEL->call_event(ON_IDLE);
- if(THEKERNEL->is_halted()) return;
- }
-
- // Start moving the axes to the origin slowly
- this->status = MOVING_TO_ENDSTOP_SLOW;
- this->feed_rate[X_AXIS] = slow_rate;
- STEPPER[X_AXIS]->move(dirx, 10000000, 0);
- this->feed_rate[Y_AXIS] = slow_rate;
- STEPPER[Y_AXIS]->move(diry, 10000000, 0);
-
- // wait for primary axis
- if(!this->wait_for_homed_corexy(home_axis)) return;
-}
-
-// this homing works for HBots/CoreXY
-void Endstops::do_homing_corexy(char axes_to_move)
-{
- // TODO should really make order configurable, and select whether to allow XY to home at the same time, diagonally
- // To move XY at the same time only one motor needs to turn, determine which motor and which direction based on min or max directions
- // allow to move until an endstop triggers, then stop that motor. Speed up when moving diagonally to match X or Y speed
- // continue moving in the direction not yet triggered (which means two motors turning) until endstop hit
-
- if((axes_to_move & 0x03) == 0x03) { // both X and Y need Homing
- // determine which motor to turn and which way
- bool dirx = this->home_direction[X_AXIS];
- bool diry = this->home_direction[Y_AXIS];
- int motor;
- bool dir;
- if(dirx && diry) { // min/min
- motor = X_AXIS;
- dir = true;
- } else if(dirx && !diry) { // min/max
- motor = Y_AXIS;
- dir = true;
- } else if(!dirx && diry) { // max/min
- motor = Y_AXIS;
- dir = false;
- } else if(!dirx && !diry) { // max/max
- motor = X_AXIS;
- dir = false;
- }
-
- // then move both X and Y until one hits the endstop
- this->status = MOVING_TO_ENDSTOP_FAST;
- // need to allow for more ground covered when moving diagonally
- this->feed_rate[motor] = this->fast_rates[motor] * 1.4142;
- STEPPER[motor]->move(dir, 10000000, 0);
- // wait until either X or Y hits the endstop
- bool running = true;
- while (running) {
- THEKERNEL->call_event(ON_IDLE);
- if(THEKERNEL->is_halted()) return;
- for(int m = X_AXIS; m <= Y_AXIS; m++) {
- if(this->pins[m + (this->home_direction[m] ? 0 : 3)].get()) {
- // turn off motor
- if(STEPPER[motor]->is_moving()) STEPPER[motor]->move(0, 0);
- running = false;
- break;
- }
- }
+ for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
+ if(axis_to_home[c]) {
+ delta[c]= this->retract_mm[c];
+ if(this->home_direction[c]) delta[c]= -delta[c];
}
}
- // move individual axis
- if (axes_to_move & 0x01) { // Home X, which means both X and Y in same direction
- bool dir = this->home_direction[X_AXIS];
- corexy_home(X_AXIS, dir, dir, this->fast_rates[X_AXIS], this->slow_rates[X_AXIS], this->retract_mm[X_AXIS]*STEPS_PER_MM(X_AXIS));
- }
-
- if (axes_to_move & 0x02) { // Home Y, which means both X and Y in different directions
- bool dir = this->home_direction[Y_AXIS];
- corexy_home(Y_AXIS, dir, !dir, this->fast_rates[Y_AXIS], this->slow_rates[Y_AXIS], this->retract_mm[Y_AXIS]*STEPS_PER_MM(Y_AXIS));
- }
-
- if (axes_to_move & 0x04) { // move Z
- do_homing_cartesian(0x04); // just home normally for Z
- }
-}
+ THEROBOT->solo_move(delta, feed_rate);
+ // wait until finished
+ THECONVEYOR->wait_for_empty_queue();
-void Endstops::home(char axes_to_move)
-{
- // not a block move so disable the last tick setting
+ // Start moving the axes to the origin slowly
+ this->status = MOVING_TO_ENDSTOP_SLOW;
for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- STEPPER[c]->set_moved_last_block(false);
- }
-
- if (is_corexy) {
- // corexy/HBot homing
- do_homing_corexy(axes_to_move);
- } else {
- // cartesian/delta homing
- do_homing_cartesian(axes_to_move);
+ delta[c]= axis_to_home[c] ? this->retract_mm[c] : 0;
}
+ THEROBOT->solo_move(delta, feed_rate);
+ // wait until finished
+ THECONVEYOR->wait_for_empty_queue();
- // make sure all steppers are off (especially if aborted)
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- STEPPER[c]->move(0, 0);
- }
this->status = NOT_HOMING;
}
} else if(gcode->subcode == 1) { // G28.1 set pre defined position
// saves current position in absolute machine coordinates
- THEKERNEL->robot->get_axis_position(saved_position);
+ THEROBOT->get_axis_position(saved_position);
return;
} else if(gcode->subcode == 3) { // G28.3 is a smoothie special it sets manual homing
if(gcode->get_num_args() == 0) {
- THEKERNEL->robot->reset_axis_position(0, 0, 0);
+ THEROBOT->reset_axis_position(0, 0, 0);
} else {
// do a manual homing based on given coordinates, no endstops required
- if(gcode->has_letter('X')) THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS);
- if(gcode->has_letter('Y')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
- if(gcode->has_letter('Z')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
+ if(gcode->has_letter('X')) THEROBOT->reset_axis_position(gcode->get_value('X'), X_AXIS);
+ if(gcode->has_letter('Y')) THEROBOT->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
+ if(gcode->has_letter('Z')) THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
}
return;
if(gcode->has_letter('A')) ac[0] = gcode->get_value('A');
if(gcode->has_letter('B')) ac[1] = gcode->get_value('B');
if(gcode->has_letter('C')) ac[2] = gcode->get_value('C');
- THEKERNEL->robot->reset_actuator_position(ac);
+ THEROBOT->reset_actuator_position(ac);
return;
} else if(THEKERNEL->is_grbl_mode()) {
// First wait for the queue to be empty
THEKERNEL->conveyor->wait_for_empty_queue();
- // Do we move select axes or all of them
- char axes_to_move = 0;
-
// deltas, scaras always home all axis
bool home_all = this->is_delta || this->is_rdelta || this->is_scara;
if(!home_all) { // ie not a delta
- bool axis_speced= ( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') );
+ bool axis_speced = ( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') );
+ axis_to_home.reset();
// only enable homing if the endstop is defined,
for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
if (this->pins[c + (this->home_direction[c] ? 0 : 3)].connected() && (!axis_speced || gcode->has_letter(c + 'X')) ) {
- axes_to_move += ( 1 << c );
+ axis_to_home.set(c);
}
}
- }else{
+ } else {
// all axis must move (and presumed defined)
- axes_to_move= 7;
+ axis_to_home.set();
}
// save current actuator position so we can report how far we moved
ActuatorCoordinates start_pos{
- THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Z_AXIS]->get_current_position()
+ THEROBOT->actuators[X_AXIS]->get_current_position(),
+ THEROBOT->actuators[Y_AXIS]->get_current_position(),
+ THEROBOT->actuators[Z_AXIS]->get_current_position()
};
// Enable the motors
- THEKERNEL->stepper->turn_enable_pins_on();
+ THEKERNEL->call_event(ON_ENABLE, (void*)1); // turn all enable pins on
// do the actual homing
if(homing_order != 0) {
+ bitset<3> axis= axis_to_home;
// if an order has been specified do it in the specified order
// homing order is 0b00ccbbaa where aa is 0,1,2 to specify the first axis, bb is the second and cc is the third
// eg 0b00100001 would be Y X Z, 0b00100100 would be X Y Z
for (uint8_t m = homing_order; m != 0; m >>= 2) {
- int a = (1 << (m & 0x03)); // axis to move
- if((a & axes_to_move) != 0) {
- home(a);
+ int a= (m & 0x03); // axis to home
+ if(axis[a]) { // if axis is selected to home
+ axis_to_home.reset();
+ axis_to_home.set(a);
+ home();
}
// check if on_halt (eg kill)
if(THEKERNEL->is_halted()) break;
}
+ axis_to_home= axis;
+
+ } else if(is_corexy) {
+ // corexy must home each axis individually
+ bitset<3> axis= axis_to_home;
+ for (int a = X_AXIS; a <= Z_AXIS; ++a) {
+ if(axis[a]) {
+ axis_to_home.reset();
+ axis_to_home.set(a);
+ home();
+ }
+ }
+ axis_to_home= axis;
} else {
- // they all home at the same time
- home(axes_to_move);
+ // they could all home at the same time
+ home();
}
// check if on_halt (eg kill)
}
// set the last probe position to the actuator units moved during this home
- THEKERNEL->robot->set_last_probe_position(
+ THEROBOT->set_last_probe_position(
std::make_tuple(
- start_pos[0] - THEKERNEL->robot->actuators[0]->get_current_position(),
- start_pos[1] - THEKERNEL->robot->actuators[1]->get_current_position(),
- start_pos[2] - THEKERNEL->robot->actuators[2]->get_current_position(),
+ start_pos[0] - THEROBOT->actuators[0]->get_current_position(),
+ start_pos[1] - THEROBOT->actuators[1]->get_current_position(),
+ start_pos[2] - THEROBOT->actuators[2]->get_current_position(),
0));
if(home_all) {
bool has_endstop_trim = this->is_delta || this->is_scara;
if (has_endstop_trim) {
ActuatorCoordinates ideal_actuator_position;
- THEKERNEL->robot->arm_solution->cartesian_to_actuator(ideal_position, ideal_actuator_position);
+ THEROBOT->arm_solution->cartesian_to_actuator(ideal_position, ideal_actuator_position);
// We are actually not at the ideal position, but a trim away
ActuatorCoordinates real_actuator_position = {
};
float real_position[3];
- THEKERNEL->robot->arm_solution->actuator_to_cartesian(real_actuator_position, real_position);
+ THEROBOT->arm_solution->actuator_to_cartesian(real_actuator_position, real_position);
// Reset the actuator positions to correspond our real position
- THEKERNEL->robot->reset_axis_position(real_position[0], real_position[1], real_position[2]);
+ THEROBOT->reset_axis_position(real_position[0], real_position[1], real_position[2]);
} else {
// without endstop trim, real_position == ideal_position
if(is_rdelta) {
// with a rotary delta we set the actuators angle then use the FK to calculate the resulting cartesian coordinates
ActuatorCoordinates real_actuator_position = {ideal_position[0], ideal_position[1], ideal_position[2]};
- THEKERNEL->robot->reset_actuator_position(real_actuator_position);
+ THEROBOT->reset_actuator_position(real_actuator_position);
} else {
// Reset the actuator positions to correspond our real position
- THEKERNEL->robot->reset_axis_position(ideal_position[0], ideal_position[1], ideal_position[2]);
+ THEROBOT->reset_axis_position(ideal_position[0], ideal_position[1], ideal_position[2]);
}
}
} else {
// Zero the ax(i/e)s position, add in the home offset
for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if ( (axes_to_move >> c) & 1 ) {
- THEKERNEL->robot->reset_axis_position(this->homing_position[c] + this->home_offset[c], c);
+ if (axis_to_home[c]) {
+ THEROBOT->reset_axis_position(this->homing_position[c] + this->home_offset[c], c);
}
}
}
// default is off for cartesian on for deltas
if(!is_delta) {
// NOTE a rotary delta usually has optical or hall-effect endstops so it is safe to go past them a little bit
- if(this->move_to_origin_after_home) move_to_origin(axes_to_move);
+ if(this->move_to_origin_after_home) move_to_origin();
// if limit switches are enabled we must back off endstop after setting home
- back_off_home(axes_to_move);
+ back_off_home();
} else if(this->move_to_origin_after_home || this->limit_enable[X_AXIS]) {
// deltas are not left at 0,0 because of the trim settings, so move to 0,0 if requested, but we need to back off endstops first
// also need to back off endstops if limits are enabled
- back_off_home(axes_to_move);
- if(this->move_to_origin_after_home) move_to_origin(axes_to_move);
+ back_off_home();
+ if(this->move_to_origin_after_home) move_to_origin();
}
}
{
// Similar to M206 and G92 but sets Homing offsets based on current position
float cartesian[3];
- THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
+ THEROBOT->get_axis_position(cartesian); // get actual position from robot
if (gcode->has_letter('X')) {
home_offset[0] -= (cartesian[X_AXIS] - gcode->get_value('X'));
- THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS);
+ THEROBOT->reset_axis_position(gcode->get_value('X'), X_AXIS);
}
if (gcode->has_letter('Y')) {
home_offset[1] -= (cartesian[Y_AXIS] - gcode->get_value('Y'));
- THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
+ THEROBOT->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
}
if (gcode->has_letter('Z')) {
home_offset[2] -= (cartesian[Z_AXIS] - gcode->get_value('Z'));
- THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
+ THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
}
gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
}
break;
- // NOTE this is to test accuracy of lead screws etc.
- case 1910: {
- // M1910.0 - move specific number of raw steps
- // M1910.1 - stop any moves
- // M1910.2 - move specific number of actuator units (usually mm but is degrees for a rotary delta)
- if(gcode->subcode == 0 || gcode->subcode == 2) {
- // Enable the motors
- THEKERNEL->stepper->turn_enable_pins_on();
-
- int32_t x = 0, y = 0, z = 0, f = 200 * 16;
- if (gcode->has_letter('F')) f = gcode->get_value('F');
-
- if (gcode->has_letter('X')) {
- float v = gcode->get_value('X');
- if(gcode->subcode == 2) x = lroundf(v * STEPS_PER_MM(X_AXIS));
- else x = roundf(v);
- STEPPER[X_AXIS]->move(x < 0, abs(x), f);
- }
- if (gcode->has_letter('Y')) {
- float v = gcode->get_value('Y');
- if(gcode->subcode == 2) y = lroundf(v * STEPS_PER_MM(Y_AXIS));
- else y = roundf(v);
- STEPPER[Y_AXIS]->move(y < 0, abs(y), f);
- }
- if (gcode->has_letter('Z')) {
- float v = gcode->get_value('Z');
- if(gcode->subcode == 2) z = lroundf(v * STEPS_PER_MM(Z_AXIS));
- else z = roundf(v);
- STEPPER[Z_AXIS]->move(z < 0, abs(z), f);
- }
- gcode->stream->printf("Moving X %ld Y %ld Z %ld steps at F %ld steps/sec\n", x, y, z, f);
-
- } else if(gcode->subcode == 1) {
- // stop any that are moving
- for (int i = 0; i < 3; ++i) {
- if(STEPPER[i]->is_moving()) STEPPER[i]->move(0, 0);
- }
- }
- break;
- }
- }
- }
-}
-
-// Called periodically to change the speed to match acceleration
-void Endstops::acceleration_tick(void)
-{
- if(this->status >= NOT_HOMING) return; // nothing to do, only do this when moving for homing sequence
-
- // foreach stepper that is moving
- for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
- if( !STEPPER[c]->is_moving() ) continue;
-
- uint32_t current_rate = STEPPER[c]->get_steps_per_second();
- uint32_t target_rate = floorf(this->feed_rate[c] * STEPS_PER_MM(c));
- float acc = (c == Z_AXIS) ? THEKERNEL->planner->get_z_acceleration() : THEKERNEL->planner->get_acceleration();
- if( current_rate < target_rate ) {
- uint32_t rate_increase = floorf((acc / THEKERNEL->acceleration_ticks_per_second) * STEPS_PER_MM(c));
- current_rate = min( target_rate, current_rate + rate_increase );
}
- if( current_rate > target_rate ) { current_rate = target_rate; }
-
- // steps per second
- STEPPER[c]->set_speed(current_rate);
}
-
- return;
}
void Endstops::on_get_public_data(void* argument)
You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
*/
-#ifndef ENDSTOPS_MODULE_H
-#define ENDSTOPS_MODULE_H
+#pragma once
#include "libs/Module.h"
#include "libs/Pin.h"
#include <bitset>
+#include <array>
class StepperMotor;
class Gcode;
Endstops();
void on_module_loaded();
void on_gcode_received(void* argument);
- void acceleration_tick(void);
private:
void load_config();
- void home(char axes_to_move);
- void do_homing_cartesian(char axes_to_move);
- void do_homing_corexy(char axes_to_move);
- bool wait_for_homed(char axes_to_move);
+ void home();
+ bool wait_for_homed();
bool wait_for_homed_corexy(int axis);
void corexy_home(int home_axis, bool dirx, bool diry, float fast_rate, float slow_rate, unsigned int retract_steps);
- void back_off_home(char axes_to_move);
- void move_to_origin(char);
+ void back_off_home();
+ void move_to_origin();
void on_get_public_data(void* argument);
void on_set_public_data(void* argument);
void on_idle(void *argument);
bool debounced_get(int pin);
void process_home_command(Gcode* gcode);
void set_homing_offset(Gcode* gcode);
+ uint32_t read_endstops(uint32_t dummy);
float homing_position[3];
float home_offset[3];
- uint8_t homing_order;
- std::bitset<3> home_direction;
- std::bitset<3> limit_enable;
float saved_position[3]{0}; // save G28 (in grbl mode)
+ float alpha_max, beta_max, gamma_max;
unsigned int debounce_count;
float retract_mm[3];
float fast_rates[3];
float slow_rates[3];
Pin pins[6];
- volatile float feed_rate[3];
+ std::array<uint16_t, 3> debounce;
+
+ std::bitset<3> home_direction;
+ std::bitset<3> limit_enable;
+ std::bitset<3> axis_to_home;
+
struct {
+ uint8_t homing_order:6;
+ uint8_t bounce_cnt:4;
+ volatile char status:3;
bool is_corexy:1;
bool is_delta:1;
bool is_rdelta:1;
bool is_scara:1;
bool move_to_origin_after_home:1;
- uint8_t bounce_cnt:4;
- volatile char status:3;
};
};
-
-#endif
int n = snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", -retract_zlift_length, retract_zlift_feedrate);
string cmd(buf, n);
Gcode gc(cmd, &(StreamOutput::NullStream));
- THEKERNEL->robot->push_state(); // save state includes feed rates etc
- THEKERNEL->robot->absolute_mode = false; // needs to be relative mode
- THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
- THEKERNEL->robot->pop_state(); // restore state includes feed rates etc
+ THEROBOT->push_state(); // save state includes feed rates etc
+ THEROBOT->absolute_mode = false; // needs to be relative mode
+ THEROBOT->on_gcode_received(&gc); // send to robot directly
+ THEROBOT->pop_state(); // restore state includes feed rates etc
}
// This is a solo move, we add an empty block to the queue to prevent subsequent gcodes being executed at the same time
int n = snprintf(buf, sizeof(buf), "G0 Z%1.4f F%1.4f", retract_zlift_length, retract_zlift_feedrate);
string cmd(buf, n);
Gcode gc(cmd, &(StreamOutput::NullStream));
- THEKERNEL->robot->push_state(); // save state includes feed rates etc
- THEKERNEL->robot->absolute_mode = false; // needs to be relative mode
- THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
- THEKERNEL->robot->pop_state(); // restore state includes feed rates etc
+ THEROBOT->push_state(); // save state includes feed rates etc
+ THEROBOT->absolute_mode = false; // needs to be relative mode
+ THEROBOT->on_gcode_received(&gc); // send to robot directly
+ THEROBOT->pop_state(); // restore state includes feed rates etc
}
} else if( this->enabled && this->retracted && (gcode->g == 0 || gcode->g == 1) && gcode->has_letter('Z')) {
// NOTE this is only used in SOLO mode, but any F on a G0/G1 will set the speed for future retracts that are not firmware retracts
if (gcode->has_letter('F')) {
- feed_rate = gcode->get_value('F') / THEKERNEL->robot->get_seconds_per_minute();
+ feed_rate = gcode->get_value('F') / THEROBOT->get_seconds_per_minute();
if (stepper_motor->get_max_rate() > 0 && feed_rate > stepper_motor->get_max_rate())
feed_rate = stepper_motor->get_max_rate();
}
ActuatorCoordinates current_angle;
// get the current angle for each actuator, NOTE we only deal with ABC so if there are more than 3 actuators this will probably go wonky
- for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
- current_angle[i]= THEKERNEL->robot->actuators[i]->get_current_position();
+ for (size_t i = 0; i < THEROBOT->actuators.size(); i++) {
+ current_angle[i]= THEROBOT->actuators[i]->get_current_position();
}
if (gcode->has_letter('L') && gcode->get_value('L') != 0) {
// specifying L1 it will take the last probe position (set by G30 or G38.x ) and set the home offset based on that
// this allows the use of G30 to find the angle tool
uint8_t ok;
- std::tie(current_angle[0], current_angle[1], current_angle[2], ok) = THEKERNEL->robot->get_last_probe_position();
+ std::tie(current_angle[0], current_angle[1], current_angle[2], ok) = THEROBOT->get_last_probe_position();
if(ok == 0) {
gcode->stream->printf("error:Nothing set as probe failed or not run\n");
return;
// reset the actuator positions (and machine position accordingly)
// But only if all three actuators have been specified at the same time
if(cnt == 3 || (gcode->has_letter('R') && gcode->get_value('R') != 0)) {
- THEKERNEL->robot->reset_actuator_position(current_angle);
+ THEROBOT->reset_actuator_position(current_angle);
gcode->stream->printf("NOTE: actuator position reset\n");
}else{
gcode->stream->printf("NOTE: actuator position NOT reset\n");
#define Y_AXIS 1
#define Z_AXIS 2
-#define STEPPER THEKERNEL->robot->actuators
+#define STEPPER THEROBOT->actuators
#define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm())
void SCARAcal::on_module_loaded()
this->get_home_offset(home_off[0], home_off[1], home_off[2]); // get home offsets
this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim
- THEKERNEL->robot->arm_solution->cartesian_to_actuator( home_off, actuator ); // convert current home offset to actuator angles
+ THEROBOT->arm_solution->cartesian_to_actuator( home_off, actuator ); // convert current home offset to actuator angles
// Subtract trim values from actuators to determine the real home offset actuator position for X and Y.
// convert back to get the real cartesian home offsets
- THEKERNEL->robot->arm_solution->actuator_to_cartesian( actuator, home_off );
+ THEROBOT->arm_solution->actuator_to_cartesian( actuator, home_off );
this->set_home_offset(home_off[0], home_off[1], home_off[2],stream); // get home offsets
// Set the correct home offsets;
actuator[2] = z;
// Calculate the physical position relating to the arm angles
- THEKERNEL->robot->arm_solution->actuator_to_cartesian( actuator, cartesian );
+ THEROBOT->arm_solution->actuator_to_cartesian( actuator, cartesian );
// Assemble Gcode to add onto the queue
snprintf(cmd, sizeof(cmd), "G0 X%1.3f Y%1.3f Z%1.3f F%1.1f", cartesian[0], cartesian[1], cartesian[2], feedrate * 60); // use specified feedrate (mm/sec)
//THEKERNEL->streams->printf("DEBUG: move: %s\n", cmd);
Gcode gc(cmd, &(StreamOutput::NullStream));
- THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
+ THEROBOT->on_gcode_received(&gc); // send to robot directly
}
//A GCode has been received
float cartesian[3];
ActuatorCoordinates actuators;
- THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
- THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
+ THEROBOT->get_axis_position(cartesian); // get actual position from robot
+ THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
int n = snprintf(buf, sizeof(buf), " A: Th:%1.3f Ps:%1.3f",
actuators[0],
float S_delta[2],
S_trim[3];
- THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
- THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
+ THEROBOT->get_axis_position(cartesian); // get actual position from robot
+ THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
S_delta[0] = actuators[0] - target[0];
} else {
set_trim(0, S_trim[1], S_trim[2], gcode->stream); // reset trim for calibration move
this->home(); // home
- THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
+ THEROBOT->get_axis_position(cartesian); // get actual position from robot
SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target
}
}
// Program the current position as target
ActuatorCoordinates actuators;
- THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
- THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
+ THEROBOT->get_axis_position(cartesian); // get actual position from robot
+ THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
STEPPER[0]->change_steps_per_mm(actuators[0] / target[0] * STEPPER[0]->get_steps_per_mm()); // Find angle difference
STEPPER[1]->change_steps_per_mm(STEPPER[0]->get_steps_per_mm()); // and change steps_per_mm to ensure correct steps per *angle*
} else {
this->home(); // home - This time leave trims as adjusted.
- THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
+ THEROBOT->get_axis_position(cartesian); // get actual position from robot
SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target
}
ActuatorCoordinates actuators;
float S_delta[2];
- THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
- THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate it to get actual actuator angles
+ THEROBOT->get_axis_position(cartesian); // get actual position from robot
+ THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate it to get actual actuator angles
S_delta[1] = ( actuators[1] - actuators[0]) - ( target[1] - target[0] ); // Find difference in angle - not actuator difference, and
set_trim(S_trim[0], S_delta[1], S_trim[2], gcode->stream); // set trim to reflect the difference
} else {
set_trim(S_trim[0], 0, S_trim[2], gcode->stream); // reset trim for calibration move
this->home(); // home
- THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
+ THEROBOT->get_axis_position(cartesian); // get actual position from robot
SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target
}
}
//send new_tool_offsets to robot
const float *new_tool_offset = tools[new_tool]->get_offset();
- THEKERNEL->robot->setToolOffset(new_tool_offset);
+ THEROBOT->setToolOffset(new_tool_offset);
}
}
}
this->current_tool_name = tool_to_add->get_name();
//send new_tool_offsets to robot
const float *new_tool_offset = tool_to_add->get_offset();
- THEKERNEL->robot->setToolOffset(new_tool_offset);
+ THEROBOT->setToolOffset(new_tool_offset);
} else {
tool_to_add->disable();
}
THEKERNEL->conveyor->wait_for_empty_queue();
// turn off any compensation transform as it will be invalidated anyway by this
- THEKERNEL->robot->compensationTransform= nullptr;
+ THEROBOT->compensationTransform= nullptr;
if(!gcode->has_letter('R')) {
if(!calibrate_delta_endstops(gcode)) {
float max_delta= 0;
float last_z= NAN;
float start_z;
- std::tie(std::ignore, std::ignore, start_z)= THEKERNEL->robot->get_axis_position();
+ std::tie(std::ignore, std::ignore, start_z)= THEROBOT->get_axis_position();
for(auto& i : pp) {
int s;
if(gcode->subcode == 0) {
gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f (%d) A:%1.4f B:%1.4f C:%1.4f\n",
i[0], i[1], z, s,
- THEKERNEL->robot->actuators[0]->get_current_position()+z,
- THEKERNEL->robot->actuators[1]->get_current_position()+z,
- THEKERNEL->robot->actuators[2]->get_current_position()+z);
+ THEROBOT->actuators[0]->get_current_position()+z,
+ THEROBOT->actuators[1]->get_current_position()+z,
+ THEROBOT->actuators[2]->get_current_position()+z);
}else if(gcode->subcode == 1) {
// format that can be pasted here http://escher3d.com/pages/wizards/wizarddelta.php
// get current delta radius
float delta_radius = 0.0F;
BaseSolution::arm_options_t options;
- if(THEKERNEL->robot->arm_solution->get_optional(options)) {
+ if(THEROBOT->arm_solution->get_optional(options)) {
delta_radius = options['R'];
}
if(delta_radius == 0.0F) {
// set the new delta radius
options['R'] = delta_radius;
- THEKERNEL->robot->arm_solution->set_optional(options);
+ THEROBOT->arm_solution->set_optional(options);
gcode->stream->printf("Setting delta radius to: %1.4f\n", delta_radius);
zprobe->home();
{
if(on) {
// set the compensationTransform in robot
- THEKERNEL->robot->compensationTransform = [this](float target[3]) { doCompensation(target); };
+ THEROBOT->compensationTransform = [this](float target[3]) { doCompensation(target); };
} else {
// clear it
- THEKERNEL->robot->compensationTransform = nullptr;
+ THEROBOT->compensationTransform = nullptr;
}
}
if(!zprobe->run_probe(s)) return false;
// TODO if using probe then we probably need to set Z to 0 at first probe point, but take into account probe offset from head
- THEKERNEL->robot->reset_axis_position(std::get<Z_AXIS>(this->probe_offsets), Z_AXIS);
+ THEROBOT->reset_axis_position(std::get<Z_AXIS>(this->probe_offsets), Z_AXIS);
// move up to specified probe start position
zprobe->coordinated_move(NAN, NAN, zprobe->getProbeHeight(), zprobe->getSlowFeedrate()); // move to probe start position
{
if(on) {
// set the compensationTransform in robot
- THEKERNEL->robot->compensationTransform= [this](float target[3]) { target[2] += this->plane->getz(target[0], target[1]); };
+ THEROBOT->compensationTransform= [this](float target[3]) { target[2] += this->plane->getz(target[0], target[1]); };
}else{
// clear it
- THEKERNEL->robot->compensationTransform= nullptr;
+ THEROBOT->compensationTransform= nullptr;
}
}
float cartesian[3];
int pindex = 0;
- THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
+ THEROBOT->get_axis_position(cartesian); // get actual position from robot
pindex = int(cartesian[X_AXIS]/this->bed_div_x + 0.25)*this->numCols + int(cartesian[Y_AXIS]/this->bed_div_y + 0.25);
{
if(on) {
// set the compensationTransform in robot
- THEKERNEL->robot->compensationTransform= [this](float target[3]) { target[2] += this->getZOffset(target[0], target[1]); };
+ THEROBOT->compensationTransform= [this](float target[3]) { target[2] += this->getZOffset(target[0], target[1]); };
}else{
// clear it
- THEKERNEL->robot->compensationTransform= nullptr;
+ THEROBOT->compensationTransform= nullptr;
}
}
#define Y_AXIS 1
#define Z_AXIS 2
-#define STEPPER THEKERNEL->robot->actuators
+#define STEPPER THEROBOT->actuators
#define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm())
#define Z_STEPS_PER_MM STEPS_PER_MM(Z_AXIS)
gcode->stream->printf("Z:%1.4f C:%d\n", zsteps_to_mm(steps), steps);
// set the last probe position to the current actuator units
- THEKERNEL->robot->set_last_probe_position(std::make_tuple(
- THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Z_AXIS]->get_current_position(),
+ THEROBOT->set_last_probe_position(std::make_tuple(
+ THEROBOT->actuators[X_AXIS]->get_current_position(),
+ THEROBOT->actuators[Y_AXIS]->get_current_position(),
+ THEROBOT->actuators[Z_AXIS]->get_current_position(),
1));
// move back to where it started, unless a Z is specified (and not a rotary delta)
if(gcode->has_letter('Z') && !is_rdelta) {
// set Z to the specified value, and leave probe where it is
- THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
+ THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
} else {
// return to pre probe position
} else {
gcode->stream->printf("ZProbe not triggered\n");
- THEKERNEL->robot->set_last_probe_position(std::make_tuple(
- THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Z_AXIS]->get_current_position(),
+ THEROBOT->set_last_probe_position(std::make_tuple(
+ THEROBOT->actuators[X_AXIS]->get_current_position(),
+ THEROBOT->actuators[Y_AXIS]->get_current_position(),
+ THEROBOT->actuators[Z_AXIS]->get_current_position(),
0));
}
THEKERNEL->conveyor->wait_for_empty_queue();
// turn off any compensation transform
- auto savect= THEKERNEL->robot->compensationTransform;
- THEKERNEL->robot->compensationTransform= nullptr;
+ auto savect= THEROBOT->compensationTransform;
+ THEROBOT->compensationTransform= nullptr;
if(gcode->has_letter('X')) {
// probe in the X axis
}
// restore compensationTransform
- THEKERNEL->robot->compensationTransform= savect;
+ THEROBOT->compensationTransform= savect;
return;
if(this->pin.get()) {
probe_detected= true;
// now tell all the stepper_motors to stop
- for(auto &a : THEKERNEL->robot->actuators) a->force_finish_move();
+ for(auto &a : THEROBOT->actuators) a->force_finish_move();
}
return 0;
}
// enable the probe checking in the timer
probing= true;
probe_detected= false;
- THEKERNEL->robot->disable_segmentation= true; // we must disable segmentation as this won't work with it enabled (beware on deltas probing in X or Y)
+ THEROBOT->disable_segmentation= true; // we must disable segmentation as this won't work with it enabled (beware on deltas probing in X or Y)
// get probe feedrate if specified
float rate = (gcode->has_letter('F')) ? gcode->get_value('F')*60 : this->slow_feedrate;
// disable probe checking
probing= false;
- THEKERNEL->robot->disable_segmentation= false;
+ THEROBOT->disable_segmentation= false;
float pos[3];
{
// get the current position
ActuatorCoordinates current_position{
- THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Z_AXIS]->get_current_position()
+ THEROBOT->actuators[X_AXIS]->get_current_position(),
+ THEROBOT->actuators[Y_AXIS]->get_current_position(),
+ THEROBOT->actuators[Z_AXIS]->get_current_position()
};
// get machine position from the actuator position using FK
- THEKERNEL->robot->arm_solution->actuator_to_cartesian(current_position, pos);
+ THEROBOT->arm_solution->actuator_to_cartesian(current_position, pos);
}
uint8_t probeok= this->probe_detected ? 1 : 0;
// print results using the GRBL format
gcode->stream->printf("[PRB:%1.3f,%1.3f,%1.3f:%d]\n", pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok);
- THEKERNEL->robot->set_last_probe_position(std::make_tuple(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok));
+ THEROBOT->set_last_probe_position(std::make_tuple(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok));
if(!probeok && gcode->subcode == 2) {
// issue error if probe was not triggered and subcode == 2
}else if(probeok){
// if the probe stopped the move we need to correct the last_milestone as it did not reach where it thought
- THEKERNEL->robot->reset_position_from_current_actuator_position();
+ THEROBOT->reset_position_from_current_actuator_position();
}
}
// also reset the steps/mm
int a= designator-'A';
if(a >= 0 && a <=2) {
- float s= THEKERNEL->robot->actuators[a]->get_steps_per_mm()*((float)microsteps/current_microsteps);
- THEKERNEL->robot->actuators[a]->change_steps_per_mm(s);
+ float s= THEROBOT->actuators[a]->get_steps_per_mm()*((float)microsteps/current_microsteps);
+ THEROBOT->actuators[a]->change_steps_per_mm(s);
gcode->stream->printf("steps/mm for %c changed to: %f\n", designator, s);
- THEKERNEL->robot->check_max_actuator_speeds();
+ THEROBOT->check_max_actuator_speeds();
}
}
microstep_override= true;
} else {
// TODO hardcoded for X need to select ABC as needed
- bool moving = THEKERNEL->robot->actuators[0]->is_moving();
+ bool moving = THEROBOT->actuators[0]->is_moving();
// dump out in the format that the processing script needs
if (moving) {
- stream->printf("#sg%d,p%lu,k%u,r,", getCurrentStallGuardReading(), THEKERNEL->robot->actuators[0]->get_current_step(), getCoolstepCurrent());
+ stream->printf("#sg%d,p%lu,k%u,r,", getCurrentStallGuardReading(), THEROBOT->actuators[0]->get_current_step(), getCoolstepCurrent());
} else {
readStatus(TMC26X_READOUT_POSITION); // get the status bits
stream->printf("#s,");
}
- stream->printf("d%d,", THEKERNEL->robot->actuators[0]->which_direction() ? 1 : -1);
+ stream->printf("d%d,", THEROBOT->actuators[0]->which_direction() ? 1 : -1);
stream->printf("c%u,m%d,", getCurrent(), getMicrosteps());
// stream->printf('S');
// stream->printf(tmc26XStepper.getSpeed(), DEC);
void PanelScreen::get_current_pos(float *cp)
{
- Robot::wcs_t mpos= THEKERNEL->robot->get_axis_position();
- Robot::wcs_t pos= THEKERNEL->robot->mcs2wcs(mpos);
- cp[0]= THEKERNEL->robot->from_millimeters(std::get<X_AXIS>(pos));
- cp[1]= THEKERNEL->robot->from_millimeters(std::get<Y_AXIS>(pos));
- cp[2]= THEKERNEL->robot->from_millimeters(std::get<Z_AXIS>(pos));
+ Robot::wcs_t mpos= THEROBOT->get_axis_position();
+ Robot::wcs_t pos= THEROBOT->mcs2wcs(mpos);
+ cp[0]= THEROBOT->from_millimeters(std::get<X_AXIS>(pos));
+ cp[1]= THEROBOT->from_millimeters(std::get<Y_AXIS>(pos));
+ cp[2]= THEROBOT->from_millimeters(std::get<Z_AXIS>(pos));
}
void PanelScreen::on_main_loop()
// steps/mm
mvs->addMenuItem("X steps/mm",
- []() -> float { return THEKERNEL->robot->actuators[0]->get_steps_per_mm(); },
- [](float v) { THEKERNEL->robot->actuators[0]->change_steps_per_mm(v); },
+ []() -> float { return THEROBOT->actuators[0]->get_steps_per_mm(); },
+ [](float v) { THEROBOT->actuators[0]->change_steps_per_mm(v); },
0.1F,
1.0F
);
mvs->addMenuItem("Y steps/mm",
- []() -> float { return THEKERNEL->robot->actuators[1]->get_steps_per_mm(); },
- [](float v) { THEKERNEL->robot->actuators[1]->change_steps_per_mm(v); },
+ []() -> float { return THEROBOT->actuators[1]->get_steps_per_mm(); },
+ [](float v) { THEROBOT->actuators[1]->change_steps_per_mm(v); },
0.1F,
1.0F
);
mvs->addMenuItem("Z steps/mm",
- []() -> float { return THEKERNEL->robot->actuators[2]->get_steps_per_mm(); },
- [](float v) { THEKERNEL->robot->actuators[2]->change_steps_per_mm(v); },
+ []() -> float { return THEROBOT->actuators[2]->get_steps_per_mm(); },
+ [](float v) { THEROBOT->actuators[2]->change_steps_per_mm(v); },
0.1F,
1.0F
);
float WatchScreen::get_current_speed()
{
// in percent
- return 6000.0F / THEKERNEL->robot->get_seconds_per_minute();
+ return 6000.0F / THEROBOT->get_seconds_per_minute();
}
void WatchScreen::get_sd_play_info()
// change pos by issuing a G0 Xnnn in absolute mode
char buf[32];
// make sure we are in absolute mode
- THEKERNEL->robot->push_state();
- THEKERNEL->robot->absolute_mode= true;
- int n = snprintf(buf, sizeof(buf), "G0 %c%f F%d", axis, p, (int)round(THEKERNEL->robot->from_millimeters(THEPANEL->get_jogging_speed(axis))));
+ THEROBOT->push_state();
+ THEROBOT->absolute_mode= true;
+ int n = snprintf(buf, sizeof(buf), "G0 %c%f F%d", axis, p, (int)round(THEROBOT->from_millimeters(THEPANEL->get_jogging_speed(axis))));
string g(buf, n);
send_gcode(g);
- THEKERNEL->robot->pop_state();
+ THEROBOT->pop_state();
}
if ( THEPANEL->click() ) {
switch(mode) {
case JOG:
- THEKERNEL->robot->reset_position_from_current_actuator_position(); // needed as we were changing the actuator only
+ THEROBOT->reset_position_from_current_actuator_position(); // needed as we were changing the actuator only
this->enter_menu_control();
this->refresh_menu();
break;
{
// get real time positions
ActuatorCoordinates current_position{
- THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Z_AXIS]->get_current_position()
+ THEROBOT->actuators[X_AXIS]->get_current_position(),
+ THEROBOT->actuators[Y_AXIS]->get_current_position(),
+ THEROBOT->actuators[Z_AXIS]->get_current_position()
};
// get machine position from the actuator position using FK
float mpos[3];
- THEKERNEL->robot->arm_solution->actuator_to_cartesian(current_position, mpos);
- Robot::wcs_t wpos= THEKERNEL->robot->mcs2wcs(mpos);
- this->pos[0]= THEKERNEL->robot->from_millimeters(std::get<X_AXIS>(wpos));
- this->pos[1]= THEKERNEL->robot->from_millimeters(std::get<Y_AXIS>(wpos));
- this->pos[2]= THEKERNEL->robot->from_millimeters(std::get<Z_AXIS>(wpos));
+ THEROBOT->arm_solution->actuator_to_cartesian(current_position, mpos);
+ Robot::wcs_t wpos= THEROBOT->mcs2wcs(mpos);
+ this->pos[0]= THEROBOT->from_millimeters(std::get<X_AXIS>(wpos));
+ this->pos[1]= THEROBOT->from_millimeters(std::get<Y_AXIS>(wpos));
+ this->pos[2]= THEROBOT->from_millimeters(std::get<Z_AXIS>(wpos));
}
// encoder tick, called in an interrupt everytime we get an encoder tick
{
for (int i = 0; i < multiplier; ++i) {
if(i != 0) wait_us(100);
- THEKERNEL->robot->actuators[axis]->manual_step(change < 0);
+ THEROBOT->actuators[axis]->manual_step(change < 0);
}
pos_changed= true;
}
// steps/mm
mvs->addMenuItem("X steps/mm",
- []() -> float { return THEKERNEL->robot->actuators[0]->get_steps_per_mm(); },
- [](float v) { THEKERNEL->robot->actuators[0]->change_steps_per_mm(v); },
+ []() -> float { return THEROBOT->actuators[0]->get_steps_per_mm(); },
+ [](float v) { THEROBOT->actuators[0]->change_steps_per_mm(v); },
0.1F,
1.0F
);
mvs->addMenuItem("Y steps/mm",
- []() -> float { return THEKERNEL->robot->actuators[1]->get_steps_per_mm(); },
- [](float v) { THEKERNEL->robot->actuators[1]->change_steps_per_mm(v); },
+ []() -> float { return THEROBOT->actuators[1]->get_steps_per_mm(); },
+ [](float v) { THEROBOT->actuators[1]->change_steps_per_mm(v); },
0.1F,
1.0F
);
mvs->addMenuItem("Z steps/mm",
- []() -> float { return THEKERNEL->robot->actuators[2]->get_steps_per_mm(); },
- [](float v) { THEKERNEL->robot->actuators[2]->change_steps_per_mm(v); },
+ []() -> float { return THEROBOT->actuators[2]->get_steps_per_mm(); },
+ [](float v) { THEROBOT->actuators[2]->change_steps_per_mm(v); },
0.1F,
1.0F
);
// get real time positions
// current actuator position in mm
ActuatorCoordinates current_position{
- THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
- THEKERNEL->robot->actuators[Z_AXIS]->get_current_position()
+ THEROBOT->actuators[X_AXIS]->get_current_position(),
+ THEROBOT->actuators[Y_AXIS]->get_current_position(),
+ THEROBOT->actuators[Z_AXIS]->get_current_position()
};
// get machine position from the actuator position using FK
float mpos[3];
- THEKERNEL->robot->arm_solution->actuator_to_cartesian(current_position, mpos);
- Robot::wcs_t wpos= THEKERNEL->robot->mcs2wcs(mpos);
- this->wpos[0]= THEKERNEL->robot->from_millimeters(std::get<X_AXIS>(wpos));
- this->wpos[1]= THEKERNEL->robot->from_millimeters(std::get<Y_AXIS>(wpos));
- this->wpos[2]= THEKERNEL->robot->from_millimeters(std::get<Z_AXIS>(wpos));
- this->mpos[0]= THEKERNEL->robot->from_millimeters(mpos[0]);
- this->mpos[1]= THEKERNEL->robot->from_millimeters(mpos[1]);
- this->mpos[2]= THEKERNEL->robot->from_millimeters(mpos[2]);
+ THEROBOT->arm_solution->actuator_to_cartesian(current_position, mpos);
+ Robot::wcs_t wpos= THEROBOT->mcs2wcs(mpos);
+ this->wpos[0]= THEROBOT->from_millimeters(std::get<X_AXIS>(wpos));
+ this->wpos[1]= THEROBOT->from_millimeters(std::get<Y_AXIS>(wpos));
+ this->wpos[2]= THEROBOT->from_millimeters(std::get<Z_AXIS>(wpos));
+ this->mpos[0]= THEROBOT->from_millimeters(mpos[0]);
+ this->mpos[1]= THEROBOT->from_millimeters(mpos[1]);
+ this->mpos[2]= THEROBOT->from_millimeters(mpos[2]);
- std::vector<Robot::wcs_t> v= THEKERNEL->robot->get_wcs_state();
+ std::vector<Robot::wcs_t> v= THEROBOT->get_wcs_state();
char current_wcs= std::get<0>(v[0]);
this->wcs= wcs2gcode(current_wcs);
}
float WatchScreen::get_current_speed()
{
// in percent
- return 6000.0F / THEKERNEL->robot->get_seconds_per_minute();
+ return 6000.0F / THEROBOT->get_seconds_per_minute();
}
void WatchScreen::get_sd_play_info()
{
// in menu mode
switch ( line ) {
- case 0: THEPANEL->lcd->printf(" WCS MCS %s", THEKERNEL->robot->inch_mode ? "in" : "mm"); break;
+ case 0: THEPANEL->lcd->printf(" WCS MCS %s", THEROBOT->inch_mode ? "in" : "mm"); break;
case 1: THEPANEL->lcd->printf("X %8.3f %8.3f", wpos[0], mpos[0]); break;
case 2: THEPANEL->lcd->printf("Y %8.3f %8.3f", wpos[1], mpos[1]); break;
case 3: THEPANEL->lcd->printf("Z %8.3f %8.3f", wpos[2], mpos[2]); break;
case 4: THEPANEL->lcd->printf("%s F%6.1f/%6.1f", this->wcs.c_str(), // display requested feedrate and actual feedrate
- THEKERNEL->robot->from_millimeters(THEKERNEL->robot->get_feed_rate()),
- THEKERNEL->robot->from_millimeters(THEKERNEL->conveyor->get_current_feedrate()*60.0F));
+ THEROBOT->from_millimeters(THEROBOT->get_feed_rate()),
+ THEROBOT->from_millimeters(THEKERNEL->conveyor->get_current_feedrate()*60.0F));
break;
case 5: THEPANEL->lcd->printf("%3d%% %2lu:%02lu %3u%% sd", this->current_speed, this->elapsed_time / 60, this->elapsed_time % 60, this->sd_pcnt_played); break;
case 6: THEPANEL->lcd->printf("%19s", this->get_status()); break;
if(this->suspended) {
// clean up
this->suspended= false;
- THEKERNEL->robot->pop_state();
+ THEROBOT->pop_state();
this->saved_temperatures.clear();
this->was_playing_file= false;
this->suspend_loops= 0;
THEKERNEL->conveyor->flush_queue();
// now the position will think it is at the last received pos, so we need to do FK to get the actuator position and reset the current position
- THEKERNEL->robot->reset_position_from_current_actuator_position();
+ THEROBOT->reset_position_from_current_actuator_position();
}
stream->printf("Aborted playing or paused file. Please turn any heaters off manually\r\n");
}
THEKERNEL->streams->printf("// Saving current state...\n");
// save current XYZ position
- THEKERNEL->robot->get_axis_position(this->saved_position);
+ THEROBOT->get_axis_position(this->saved_position);
// save current extruder state
PublicData::set_value( extruder_checksum, save_state_checksum, nullptr );
// save state use M120
- THEKERNEL->robot->push_state();
+ THEROBOT->push_state();
// TODO retract by optional amount...
if(THEKERNEL->is_halted()) {
// abort temp wait and rest of resume
THEKERNEL->streams->printf("Resume aborted by kill\n");
- THEKERNEL->robot->pop_state();
+ THEROBOT->pop_state();
this->saved_temperatures.clear();
suspended= false;
return;
// Restore position
stream->printf("Restoring saved XYZ positions and state...\n");
- THEKERNEL->robot->pop_state();
- bool abs_mode= THEKERNEL->robot->absolute_mode; // what mode we were in
+ THEROBOT->pop_state();
+ bool abs_mode= THEROBOT->absolute_mode; // what mode we were in
// force absolute mode for restoring position, then set to the saved relative/absolute mode
- THEKERNEL->robot->absolute_mode= true;
+ THEROBOT->absolute_mode= true;
{
// NOTE position was saved in MCS so must use G53 to restore position
char buf[128];
message.stream = &(StreamOutput::NullStream);
THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
}
- THEKERNEL->robot->absolute_mode= abs_mode;
+ THEROBOT->absolute_mode= abs_mode;
// restore extruder state
PublicData::set_value( extruder_checksum, restore_state_checksum, nullptr );
bool verbose = shift_parameter( parameters ).find_first_of("Vv") != string::npos;
- std::vector<Robot::wcs_t> v= THEKERNEL->robot->get_wcs_state();
+ std::vector<Robot::wcs_t> v= THEROBOT->get_wcs_state();
if(verbose) {
char current_wcs= std::get<0>(v[0]);
stream->printf("[current WCS: %s]\n", wcs2gcode(current_wcs).c_str());
int n= std::get<1>(v[0]);
for (int i = 1; i <= n; ++i) {
stream->printf("[%s:%1.4f,%1.4f,%1.4f]\n", wcs2gcode(i-1).c_str(),
- THEKERNEL->robot->from_millimeters(std::get<0>(v[i])),
- THEKERNEL->robot->from_millimeters(std::get<1>(v[i])),
- THEKERNEL->robot->from_millimeters(std::get<2>(v[i])));
+ THEROBOT->from_millimeters(std::get<0>(v[i])),
+ THEROBOT->from_millimeters(std::get<1>(v[i])),
+ THEROBOT->from_millimeters(std::get<2>(v[i])));
}
float *rd;
PublicData::get_value( endstops_checksum, saved_position_checksum, &rd );
stream->printf("[G28:%1.4f,%1.4f,%1.4f]\n",
- THEKERNEL->robot->from_millimeters(rd[0]),
- THEKERNEL->robot->from_millimeters(rd[1]),
- THEKERNEL->robot->from_millimeters(rd[2]));
+ THEROBOT->from_millimeters(rd[0]),
+ THEROBOT->from_millimeters(rd[1]),
+ THEROBOT->from_millimeters(rd[2]));
stream->printf("[G30:%1.4f,%1.4f,%1.4f]\n", 0.0F, 0.0F, 0.0F); // not implemented
stream->printf("[G92:%1.4f,%1.4f,%1.4f]\n",
- THEKERNEL->robot->from_millimeters(std::get<0>(v[n+1])),
- THEKERNEL->robot->from_millimeters(std::get<1>(v[n+1])),
- THEKERNEL->robot->from_millimeters(std::get<2>(v[n+1])));
+ THEROBOT->from_millimeters(std::get<0>(v[n+1])),
+ THEROBOT->from_millimeters(std::get<1>(v[n+1])),
+ THEROBOT->from_millimeters(std::get<2>(v[n+1])));
if(verbose) {
stream->printf("[Tool Offset:%1.4f,%1.4f,%1.4f]\n",
- THEKERNEL->robot->from_millimeters(std::get<0>(v[n+2])),
- THEKERNEL->robot->from_millimeters(std::get<1>(v[n+2])),
- THEKERNEL->robot->from_millimeters(std::get<2>(v[n+2])));
+ THEROBOT->from_millimeters(std::get<0>(v[n+2])),
+ THEROBOT->from_millimeters(std::get<1>(v[n+2])),
+ THEROBOT->from_millimeters(std::get<2>(v[n+2])));
}else{
- stream->printf("[TL0:%1.4f]\n", THEKERNEL->robot->from_millimeters(std::get<2>(v[n+2])));
+ stream->printf("[TL0:%1.4f]\n", THEROBOT->from_millimeters(std::get<2>(v[n+2])));
}
// this is the last probe position, updated when a probe completes, also stores the number of steps moved after a homing cycle
float px, py, pz;
uint8_t ps;
- std::tie(px, py, pz, ps) = THEKERNEL->robot->get_last_probe_position();
- stream->printf("[PRB:%1.4f,%1.4f,%1.4f:%d]\n", THEKERNEL->robot->from_millimeters(px), THEKERNEL->robot->from_millimeters(py), THEKERNEL->robot->from_millimeters(pz), ps);
+ std::tie(px, py, pz, ps) = THEROBOT->get_last_probe_position();
+ stream->printf("[PRB:%1.4f,%1.4f,%1.4f:%d]\n", THEROBOT->from_millimeters(px), THEROBOT->from_millimeters(py), THEROBOT->from_millimeters(pz), ps);
}
void SimpleShell::get_command( string parameters, StreamOutput *stream)
// do forward kinematics on the given actuator position and display the cartesian coordinates
ActuatorCoordinates apos{x, y, z};
float pos[3];
- THEKERNEL->robot->arm_solution->actuator_to_cartesian(apos, pos);
+ THEROBOT->arm_solution->actuator_to_cartesian(apos, pos);
stream->printf("cartesian= X %f, Y %f, Z %f, Steps= A %lu, B %lu, C %lu\n",
pos[0], pos[1], pos[2],
- lroundf(x*THEKERNEL->robot->actuators[0]->get_steps_per_mm()),
- lroundf(y*THEKERNEL->robot->actuators[1]->get_steps_per_mm()),
- lroundf(z*THEKERNEL->robot->actuators[2]->get_steps_per_mm()));
+ lroundf(x*THEROBOT->actuators[0]->get_steps_per_mm()),
+ lroundf(y*THEROBOT->actuators[1]->get_steps_per_mm()),
+ lroundf(z*THEROBOT->actuators[2]->get_steps_per_mm()));
x= pos[0];
y= pos[1];
z= pos[2];
// do inverse kinematics on the given cartesian position and display the actuator coordinates
float pos[3]{x, y, z};
ActuatorCoordinates apos;
- THEKERNEL->robot->arm_solution->cartesian_to_actuator(pos, apos);
+ THEROBOT->arm_solution->cartesian_to_actuator(pos, apos);
stream->printf("actuator= A %f, B %f, C %f\n", apos[0], apos[1], apos[2]);
}
} else if (what == "pos") {
// convenience to call all the various M114 variants
char buf[64];
- THEKERNEL->robot->print_position(0, buf, sizeof buf); stream->printf("last %s\n", buf);
- THEKERNEL->robot->print_position(1, buf, sizeof buf); stream->printf("realtime %s\n", buf);
- THEKERNEL->robot->print_position(2, buf, sizeof buf); stream->printf("%s\n", buf);
- THEKERNEL->robot->print_position(3, buf, sizeof buf); stream->printf("%s\n", buf);
- THEKERNEL->robot->print_position(4, buf, sizeof buf); stream->printf("%s\n", buf);
- THEKERNEL->robot->print_position(5, buf, sizeof buf); stream->printf("%s\n", buf);
+ THEROBOT->print_position(0, buf, sizeof buf); stream->printf("last %s\n", buf);
+ THEROBOT->print_position(1, buf, sizeof buf); stream->printf("realtime %s\n", buf);
+ THEROBOT->print_position(2, buf, sizeof buf); stream->printf("%s\n", buf);
+ THEROBOT->print_position(3, buf, sizeof buf); stream->printf("%s\n", buf);
+ THEROBOT->print_position(4, buf, sizeof buf); stream->printf("%s\n", buf);
+ THEROBOT->print_position(5, buf, sizeof buf); stream->printf("%s\n", buf);
} else if (what == "wcs") {
// print the wcs state
// [G0 G54 G17 G21 G90 G94 M0 M5 M9 T0 F0.]
stream->printf("[G%d %s G%d G%d G%d G94 M0 M5 M9 T%d F%1.4f]\n",
THEKERNEL->gcode_dispatch->get_modal_command(),
- wcs2gcode(THEKERNEL->robot->get_current_wcs()).c_str(),
- THEKERNEL->robot->plane_axis_0 == X_AXIS && THEKERNEL->robot->plane_axis_1 == Y_AXIS && THEKERNEL->robot->plane_axis_2 == Z_AXIS ? 17 :
- THEKERNEL->robot->plane_axis_0 == X_AXIS && THEKERNEL->robot->plane_axis_1 == Z_AXIS && THEKERNEL->robot->plane_axis_2 == Y_AXIS ? 18 :
- THEKERNEL->robot->plane_axis_0 == Y_AXIS && THEKERNEL->robot->plane_axis_1 == Z_AXIS && THEKERNEL->robot->plane_axis_2 == X_AXIS ? 19 : 17,
- THEKERNEL->robot->inch_mode ? 20 : 21,
- THEKERNEL->robot->absolute_mode ? 90 : 91,
+ wcs2gcode(THEROBOT->get_current_wcs()).c_str(),
+ THEROBOT->plane_axis_0 == X_AXIS && THEROBOT->plane_axis_1 == Y_AXIS && THEROBOT->plane_axis_2 == Z_AXIS ? 17 :
+ THEROBOT->plane_axis_0 == X_AXIS && THEROBOT->plane_axis_1 == Z_AXIS && THEROBOT->plane_axis_2 == Y_AXIS ? 18 :
+ THEROBOT->plane_axis_0 == Y_AXIS && THEROBOT->plane_axis_1 == Z_AXIS && THEROBOT->plane_axis_2 == X_AXIS ? 19 : 17,
+ THEROBOT->inch_mode ? 20 : 21,
+ THEROBOT->absolute_mode ? 90 : 91,
get_active_tool(),
- THEKERNEL->robot->from_millimeters(THEKERNEL->robot->get_feed_rate()));
+ THEROBOT->from_millimeters(THEROBOT->get_feed_rate()));
} else if (what == "status") {
// also ? on serial and usb
return;
}
float d= strtof(dist.c_str(), NULL);
- float f= speed.empty() ? THEKERNEL->robot->get_feed_rate() : strtof(speed.c_str(), NULL);
+ float f= speed.empty() ? THEROBOT->get_feed_rate() : strtof(speed.c_str(), NULL);
uint32_t n= strtol(iters.c_str(), NULL, 10);
bool toggle= false;
float r= strtof(radius.c_str(), NULL);
uint32_t n= strtol(iters.c_str(), NULL, 10);
- float f= speed.empty() ? THEKERNEL->robot->get_feed_rate() : strtof(speed.c_str(), NULL);
+ float f= speed.empty() ? THEROBOT->get_feed_rate() : strtof(speed.c_str(), NULL);
char cmd[64];
snprintf(cmd, sizeof(cmd), "G0 X%f Y0 F%f", -r, f);
return;
}
float d= strtof(size.c_str(), NULL);
- float f= speed.empty() ? THEKERNEL->robot->get_feed_rate() : strtof(speed.c_str(), NULL);
+ float f= speed.empty() ? THEROBOT->get_feed_rate() : strtof(speed.c_str(), NULL);
uint32_t n= strtol(iters.c_str(), NULL, 10);
for (uint32_t i = 0; i < n; ++i) {
HCoop / clinton / Smoothieware.git / commitdiff