From 7d6fe308987a3cf517f0f18783aefa3354b38494 Mon Sep 17 00:00:00 2001 From: Jim Morris Date: Sun, 4 May 2014 22:34:21 -0700 Subject: [PATCH] add public set for endstop trims tweak zprobe and avoid potential race condition --- src/modules/tools/endstops/Endstops.cpp | 15 ++++ src/modules/tools/endstops/Endstops.h | 11 ++- src/modules/tools/zprobe/ZProbe.cpp | 110 ++++++++++++++---------- src/modules/tools/zprobe/ZProbe.h | 9 +- 4 files changed, 93 insertions(+), 52 deletions(-) diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp index eafa7563..3ba467ce 100644 --- a/src/modules/tools/endstops/Endstops.cpp +++ b/src/modules/tools/endstops/Endstops.cpp @@ -114,6 +114,7 @@ void Endstops::on_module_loaded() register_for_event(ON_CONFIG_RELOAD); register_for_event(ON_GCODE_RECEIVED); register_for_event(ON_GET_PUBLIC_DATA); + register_for_event(ON_SET_PUBLIC_DATA); // Take StepperMotor objects from Robot and keep them here this->steppers[0] = THEKERNEL->robot->alpha_stepper_motor; @@ -592,3 +593,17 @@ void Endstops::on_get_public_data(void* argument){ pdr->set_taken(); } } + +void Endstops::on_set_public_data(void* argument){ + PublicDataRequest* pdr = static_cast(argument); + + if(!pdr->starts_with(endstops_checksum)) return; + + if(pdr->second_element_is(trim_checksum)) { + float *t= static_cast(pdr->get_data_ptr()); + this->trim_mm[0]= t[0]; + this->trim_mm[1]= t[1]; + this->trim_mm[2]= t[2]; + pdr->set_taken(); + } +} diff --git a/src/modules/tools/endstops/Endstops.h b/src/modules/tools/endstops/Endstops.h index 0692128c..30c97433 100644 --- a/src/modules/tools/endstops/Endstops.h +++ b/src/modules/tools/endstops/Endstops.h @@ -11,6 +11,8 @@ #include "libs/Module.h" #include "libs/Pin.h" +#include + class StepperMotor; class Endstops : public Module{ @@ -28,10 +30,11 @@ class Endstops : public Module{ void 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 on_get_public_data(void* argument); + void on_set_public_data(void* argument); float homing_position[3]; float home_offset[3]; - bool home_direction[3]; + std::bitset<3> home_direction; unsigned int debounce_count; float retract_mm[3]; float trim_mm[3]; @@ -41,8 +44,10 @@ class Endstops : public Module{ Pin pins[6]; StepperMotor* steppers[3]; char status; - bool is_corexy; - bool is_delta; + struct { + bool is_corexy:1; + bool is_delta:1; + }; }; #endif diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp index f0c24fb1..5be0c3e6 100644 --- a/src/modules/tools/zprobe/ZProbe.cpp +++ b/src/modules/tools/zprobe/ZProbe.cpp @@ -47,6 +47,8 @@ #define STEPS_PER_MM(a) (this->steppers[a]->steps_per_mm) #define Z_STEPS_PER_MM STEPS_PER_MM(Z_AXIS) +#define abs(a) ((a<0) ? -a : a) + void ZProbe::on_module_loaded() { // if the module is disabled -> do nothing @@ -135,7 +137,6 @@ bool ZProbe::run_probe(int& steps, bool fast) this->current_feedrate = (fast ? this->fast_feedrate : this->slow_feedrate) * Z_STEPS_PER_MM; // steps/sec // move Z down - this->running = true; this->steppers[Z_AXIS]->set_speed(0); // will be increased by acceleration tick this->steppers[Z_AXIS]->move(true, 1000 * Z_STEPS_PER_MM); // always probes down, no more than 1000mm TODO should be 2*maxz if(this->is_delta) { @@ -146,6 +147,8 @@ bool ZProbe::run_probe(int& steps, bool fast) this->steppers[Y_AXIS]->move(true, 1000 * STEPS_PER_MM(Y_AXIS)); } + this->running = true; + int s[3]; bool r = wait_for_probe(s); steps= s[Z_AXIS]; // only need z @@ -160,7 +163,6 @@ bool ZProbe::return_probe(int steps) bool dir= steps < 0; steps= abs(steps); - this->running = true; this->steppers[Z_AXIS]->set_speed(0); // will be increased by acceleration tick this->steppers[Z_AXIS]->move(dir, steps); if(this->is_delta) { @@ -169,6 +171,8 @@ bool ZProbe::return_probe(int steps) this->steppers[Y_AXIS]->set_speed(0); this->steppers[Y_AXIS]->move(dir, steps); } + + this->running = true; while(this->steppers[X_AXIS]->moving || this->steppers[Y_AXIS]->moving || this->steppers[Z_AXIS]->moving) { // wait for it to complete THEKERNEL->call_event(ON_IDLE); @@ -232,19 +236,12 @@ bool ZProbe::calibrate_delta_endstops(Gcode *gcode) float trimx= 0.0F, trimy= 0.0F, trimz= 0.0F; if(!keep) { // zero trim values - set_trim(0, 0, 0, &(StreamOutput::NullStream)); + if(!set_trim(0, 0, 0, &(StreamOutput::NullStream))) return false; }else{ // get current trim, and continue from that - void *returned_data; - bool ok = THEKERNEL->public_data->get_value( endstops_checksum, trim_checksum, &returned_data ); - - if (ok) { - float *trim = static_cast(returned_data); - gcode->stream->printf("Current Trim X: %f, Y: %f, Z: %f\r\n", trim[0], trim[1], trim[2]); - trimx= trim[0]; - trimy= trim[1]; - trimz= trim[2]; + if (get_trim(trimx, trimy, trimz)) { + gcode->stream->printf("Current Trim X: %f, Y: %f, Z: %f\r\n", trimx, trimy, trimz); } else { gcode->stream->printf("Could not get current trim, are endstops enabled?\n"); @@ -297,8 +294,7 @@ bool ZProbe::calibrate_delta_endstops(Gcode *gcode) for (int i = 1; i <= 10; ++i) { // set trim - gcode->stream->printf("Set Trim:\n"); - set_trim(trimx, trimy, trimz, gcode->stream); + if(!set_trim(trimx, trimy, trimz, gcode->stream)) return false; // home and move probe to start position just above the bed home(); @@ -368,26 +364,11 @@ bool ZProbe::calibrate_delta_radius(Gcode *gcode) home(); coordinated_move(NAN, NAN, -bedht, this->fast_feedrate, true); // do a relative move from home to the point above the bed - // probe the base of the three towers to get reference point at this Z height - int dx= 0, dy= 0, dz= 0, dc= 0; - if(!probe_delta_tower(dx, t1x, t1y)) return false; - gcode->stream->printf("T1 Z:%1.3f C:%d\n", dx / Z_STEPS_PER_MM, dx); - if(!probe_delta_tower(dy, t2x, t2y)) return false; - gcode->stream->printf("T2 Z:%1.3f C:%d\n", dy / Z_STEPS_PER_MM, dy); - if(!probe_delta_tower(dz, t3x, t3y)) return false; - gcode->stream->printf("T3 Z:%1.3f C:%d\n", dz / Z_STEPS_PER_MM, dz); + // probe center to get reference point at this Z height + int dc; if(!probe_delta_tower(dc, 0, 0)) return false; gcode->stream->printf("CT Z:%1.3f C:%d\n", dc / Z_STEPS_PER_MM, dc); - - - // See if we are already in range and skip calibration if not needed float cmm= dc / Z_STEPS_PER_MM; - float m= dx / Z_STEPS_PER_MM; - float d= cmm-m; - if(abs(d) <= target) { - gcode->stream->printf("Delta Radius already in range: %1.3f\n", d); - return true; - } // get current delta radius float delta_radius= 0.0F; @@ -401,9 +382,28 @@ bool ZProbe::calibrate_delta_radius(Gcode *gcode) } options.clear(); - // probe t1, but use coordinated moves, probing center won't change float drinc= 2.5F; // approx for (int i = 1; i <= 10; ++i) { + // probe t1, t2, t3 and get average, but use coordinated moves, probing center won't change + int dx, dy, dz; + if(!probe_delta_tower(dx, t1x, t1y)) return false; + gcode->stream->printf("T1-%d Z:%1.3f C:%d\n", i, dx / Z_STEPS_PER_MM, dx); + if(!probe_delta_tower(dy, t2x, t2y)) return false; + gcode->stream->printf("T2-%d Z:%1.3f C:%d\n", i, dy / Z_STEPS_PER_MM, dy); + if(!probe_delta_tower(dz, t3x, t3y)) return false; + gcode->stream->printf("T3-%d Z:%1.3f C:%d\n", i, dz / Z_STEPS_PER_MM, dz); + + // now look at the difference and reduce it by adjusting delta radius + float m= ((dx+dy+dz)/3.0F) / Z_STEPS_PER_MM; + float d= cmm-m; + gcode->stream->printf("C-%d Z-ave:%1.4f delta: %1.3f\n", i, m, d); + + if(abs(d) <= target) break; // resolution of success + + // increase delta radius to adjust for low center + // decrease delta radius to adjust for high center + delta_radius += (d*drinc); + // set the new delta radius options['R']= delta_radius; THEKERNEL->robot->arm_solution->set_optional(options); @@ -411,16 +411,9 @@ bool ZProbe::calibrate_delta_radius(Gcode *gcode) home(); coordinated_move(NAN, NAN, -bedht, this->fast_feedrate, true); // needs to be a relative coordinated move - if(!probe_delta_tower(dx, t1x, t1y)) return false; - // now look at the difference and reduce it by adjusting delta radius - m= dx / Z_STEPS_PER_MM; - d= cmm-m; - gcode->stream->printf("T1-%d Z:%1.4f C:%d delta: %1.3f\n", i, m, dx, d); - if(abs(d) <= target) break; // resolution of success - // increase delta radius to adjust for low center - // decrease delta radius to adjust for high center - delta_radius += (d*drinc); + // flush the output + THEKERNEL->call_event(ON_IDLE); } return true; } @@ -483,6 +476,10 @@ void ZProbe::on_gcode_received(void *argument) gcode->add_nl = true; gcode->mark_as_taken(); + } else if (gcode->m == 557) { // P0 Xxxx Yyyy sets probe points for G32 + // TODO will override the automatically calculated probe points for a delta, required for a cartesian + + gcode->mark_as_taken(); } } } @@ -560,10 +557,31 @@ void ZProbe::home() THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); } -void ZProbe::set_trim(float x, float y, float z, StreamOutput *stream) +bool ZProbe::set_trim(float x, float y, float z, StreamOutput *stream) { - char buf[40]; - int n = snprintf(buf, sizeof(buf), "M666 X%1.8f Y%1.8f Z%1.8f", x, y, z); - Gcode gc(string(buf, n), stream); - THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc); + float t[3]{x, y, z}; + bool ok= THEKERNEL->public_data->set_value( endstops_checksum, trim_checksum, t); + + if (ok) { + stream->printf("set trim to X:%f Y:%f Z:%f\n", x, y, z); + } else { + stream->printf("unable to set trim, is endstops enabled?\n"); + } + + return ok; +} + +bool ZProbe::get_trim(float& x, float& y, float& z) +{ + void *returned_data; + bool ok = THEKERNEL->public_data->get_value( endstops_checksum, trim_checksum, &returned_data ); + + if (ok) { + float *trim = static_cast(returned_data); + x= trim[0]; + y= trim[1]; + z= trim[2]; + return true; + } + return false; } diff --git a/src/modules/tools/zprobe/ZProbe.h b/src/modules/tools/zprobe/ZProbe.h index c4699ffb..bb668350 100644 --- a/src/modules/tools/zprobe/ZProbe.h +++ b/src/modules/tools/zprobe/ZProbe.h @@ -35,7 +35,8 @@ private: bool calibrate_delta_radius(Gcode *gcode); void coordinated_move(float x, float y, float z, float feedrate, bool relative=false); void home(); - void set_trim(float x, float y, float z, StreamOutput *stream); + bool set_trim(float x, float y, float z, StreamOutput *stream); + bool get_trim(float& x, float& y, float& z); float probe_radius; float probe_height; @@ -45,8 +46,10 @@ private: StepperMotor *steppers[3]; Pin pin; uint8_t debounce_count; - bool running; - bool is_delta; + struct { + bool running:1; + bool is_delta:1; + }; }; #endif /* ZPROBE_H_ */ -- 2.20.1