Added G32 to autocalibrate delta endstops

Optimized Vector3 and added a few useful functions
cleanup kossel
play comamnd should display ok and other output if -v is specified on play command

diff --git a/src/libs/Vector3.cpp b/src/libs/Vector3.cpp
index d1f4828..bc9cda5 100644 (file)
--- a/src/libs/Vector3.cpp
+++ b/src/libs/Vector3.cpp
@@ -17,14 +17,14 @@ Vector3::Vector3(float a, float b, float c)
     elem[2] = c;
 }
 
-float& Vector3::operator[](int i)
+float Vector3::operator[](int i) const
 {
     if (i >= 0 && i <= 2)
         return elem[i];
     return nan;
 }
 
-Vector3 Vector3::cross(const Vector3 vec)
+Vector3 Vector3::cross(const Vector3 &vec) const
 {
     Vector3 out;
 
@@ -35,26 +35,26 @@ Vector3 Vector3::cross(const Vector3 vec)
     return out;
 }
 
-float Vector3::dot(const Vector3 vec)
+float Vector3::dot(const Vector3 &vec) const
 {
     return  elem[0] * vec.elem[0] +
-    elem[1] * vec.elem[1] +
-    elem[2] * vec.elem[2];
+            elem[1] * vec.elem[1] +
+            elem[2] * vec.elem[2];
 }
 
-float Vector3::magsq()
+float Vector3::magsq() const
 {
     return  powf(elem[0], 2) +
-    powf(elem[1], 2) +
-    powf(elem[2], 2);
+            powf(elem[1], 2) +
+            powf(elem[2], 2);
 }
 
-float Vector3::mag()
+float Vector3::mag() const
 {
     return sqrtf(magsq());
 }
 
-Vector3 Vector3::add(const Vector3 vec)
+Vector3 Vector3::add(const Vector3 &vec) const
 {
     Vector3 out;
 
@@ -65,7 +65,7 @@ Vector3 Vector3::add(const Vector3 vec)
     return out;
 }
 
-Vector3 Vector3::sub(const Vector3 vec)
+Vector3 Vector3::sub(const Vector3 &vec) const
 {
     Vector3 out;
 
@@ -76,7 +76,7 @@ Vector3 Vector3::sub(const Vector3 vec)
     return out;
 }
 
-Vector3 Vector3::mul(float scalar)
+Vector3 Vector3::mul(float scalar) const
 {
     Vector3 out;
 
@@ -87,7 +87,18 @@ Vector3 Vector3::mul(float scalar)
     return out;
 }
 
-Vector3 Vector3::unit()
+Vector3 Vector3::mul(const Vector3& v) const
+{
+    Vector3 out;
+
+    out.elem[0] = elem[0] * v[0];
+    out.elem[1] = elem[1] * v[1];
+    out.elem[2] = elem[2] * v[2];
+
+    return out;
+}
+
+Vector3 Vector3::unit() const
 {
     Vector3 out;
 

diff --git a/src/libs/Vector3.h b/src/libs/Vector3.h
index dc376e9..6ebb9e2 100644 (file)
--- a/src/libs/Vector3.h
+++ b/src/libs/Vector3.h
@@ -7,21 +7,22 @@ public:
     Vector3();
     Vector3(float, float, float);
 
-    float&   operator[](int);
+    float    operator[](int) const;
 
-    Vector3  cross(const Vector3);
+    Vector3  cross(const Vector3&) const;
 
-    float    dot(const Vector3);
+    float    dot(const Vector3&) const;
 
-    float    magsq();
-    float    mag();
+    float    magsq() const;
+    float    mag() const;
 
-    Vector3  add(const Vector3);
-    Vector3  sub(const Vector3);
+    Vector3  add(const Vector3&) const;
+    Vector3  sub(const Vector3&) const;
 
-    Vector3  mul(float);
+    Vector3  mul(float) const;
+    Vector3  mul(const Vector3& v) const;
 
-    Vector3  unit(void);
+    Vector3  unit(void) const;
 
 private:
     float  elem[3];

diff --git a/src/modules/robot/arm_solutions/JohannKosselSolution.cpp b/src/modules/robot/arm_solutions/JohannKosselSolution.cpp
index d4c401b..dda6a28 100644 (file)
--- a/src/modules/robot/arm_solutions/JohannKosselSolution.cpp
+++ b/src/modules/robot/arm_solutions/JohannKosselSolution.cpp
@@ -2,9 +2,17 @@
 #include <fastmath.h>
 #include "checksumm.h"
 #include "ConfigValue.h"
+#include "libs/Kernel.h"
 
+#include "libs/nuts_bolts.h"
+
+#include "libs/Config.h"
 #include "Vector3.h"
 
+#define arm_length_checksum         CHECKSUM("arm_length")
+#define arm_radius_checksum         CHECKSUM("arm_radius")
+
+
 #define SQ(x) powf(x, 2)
 #define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y)
 

diff --git a/src/modules/robot/arm_solutions/JohannKosselSolution.h b/src/modules/robot/arm_solutions/JohannKosselSolution.h
index 1820876..07b24ed 100644 (file)
--- a/src/modules/robot/arm_solutions/JohannKosselSolution.h
+++ b/src/modules/robot/arm_solutions/JohannKosselSolution.h
@@ -1,14 +1,9 @@
 #ifndef JOHANNKOSSELSOLUTION_H
 #define ROSTOCKSOLUTION_H
 #include "libs/Module.h"
-#include "libs/Kernel.h"
 #include "BaseSolution.h"
-#include "libs/nuts_bolts.h"
 
-#include "libs/Config.h"
-
-#define arm_length_checksum         CHECKSUM("arm_length")
-#define arm_radius_checksum         CHECKSUM("arm_radius")
+class Config;
 
 class JohannKosselSolution : public BaseSolution {
     public:

diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp
index da94a62..ebbf5a5 100644 (file)
--- a/src/modules/tools/zprobe/ZProbe.cpp
+++ b/src/modules/tools/zprobe/ZProbe.cpp
@@ -21,13 +21,22 @@
 #include "SlowTicker.h"
 #include "Planner.h"
 
+#include <tuple>
+#include <algorithm>
+
 #define zprobe_checksum          CHECKSUM("zprobe")
 #define enable_checksum          CHECKSUM("enable")
 #define probe_pin_checksum       CHECKSUM("probe_pin")
 #define debounce_count_checksum  CHECKSUM("debounce_count")
-#define feedrate_checksum        CHECKSUM("feedrate")
+#define slow_feedrate_checksum   CHECKSUM("slow_feedrate")
+#define fast_feedrate_checksum   CHECKSUM("fast_feedrate")
+#define probe_radius_checksum    CHECKSUM("probe_radius")
+#define endstop_radius_checksum  CHECKSUM("endstop_radius")
+#define probe_height_checksum    CHECKSUM("probe_height")
 
+// from endstop section
 #define delta_homing_checksum    CHECKSUM("delta_homing")
+#define arm_radius_checksum      CHECKSUM("arm_radius")
 
 #define alpha_steps_per_mm_checksum      CHECKSUM("alpha_steps_per_mm")
 #define beta_steps_per_mm_checksum       CHECKSUM("beta_steps_per_mm")
@@ -46,7 +55,7 @@ void ZProbe::on_module_loaded()
         delete this;
         return;
     }
-    this->running= false;
+    this->running = false;
 
     // load settings
     this->on_config_reload(this);
@@ -60,26 +69,35 @@ void ZProbe::on_module_loaded()
 
 void ZProbe::on_config_reload(void *argument)
 {
-    this->pin.from_string(  THEKERNEL->config->value(zprobe_checksum, probe_pin_checksum)->by_default("nc" )->as_string())->as_input();
-    this->debounce_count =  THEKERNEL->config->value(zprobe_checksum, debounce_count_checksum)->by_default(0  )->as_number();
+    this->pin.from_string( THEKERNEL->config->value(zprobe_checksum, probe_pin_checksum)->by_default("nc" )->as_string())->as_input();
+    this->debounce_count = THEKERNEL->config->value(zprobe_checksum, debounce_count_checksum)->by_default(0  )->as_number();
+
+    // Note this could be overriden by M665
+    float delta_radius = THEKERNEL->config->value(arm_radius_checksum)->by_default(124.0f)->as_number();
+
+    // default is half the delta radius
+    this->probe_radius =  THEKERNEL->config->value(zprobe_checksum, probe_radius_checksum)->by_default(delta_radius / 2)->as_number();
+    this->endstop_radius =  THEKERNEL->config->value(zprobe_checksum, endstop_radius_checksum)->by_default(delta_radius)->as_number();
+    this->probe_height =  THEKERNEL->config->value(zprobe_checksum, probe_height_checksum)->by_default(5.0F)->as_number();
 
     this->steppers[0] = THEKERNEL->robot->alpha_stepper_motor;
     this->steppers[1] = THEKERNEL->robot->beta_stepper_motor;
     this->steppers[2] = THEKERNEL->robot->gamma_stepper_motor;
 
     // we need to know steps per mm
-    this->steps_per_mm[0] =  THEKERNEL->config->value(alpha_steps_per_mm_checksum)->as_number();
-    this->steps_per_mm[1] =  THEKERNEL->config->value(beta_steps_per_mm_checksum)->as_number();
-    this->steps_per_mm[2] =  THEKERNEL->config->value(gamma_steps_per_mm_checksum)->as_number();
+    // FIXME we need to get this after config loaded from robot as the config settings can be overriden or trap M92
+    this->steps_per_mm[0] = THEKERNEL->config->value(alpha_steps_per_mm_checksum)->as_number();
+    this->steps_per_mm[1] = THEKERNEL->config->value(beta_steps_per_mm_checksum)->as_number();
+    this->steps_per_mm[2] = THEKERNEL->config->value(gamma_steps_per_mm_checksum)->as_number();
 
-    this->feedrate = THEKERNEL->config->value(zprobe_checksum, feedrate_checksum)->by_default(5)->as_number()*this->steps_per_mm[Z_AXIS]; // feedrate in steps/sec
+    this->slow_feedrate = THEKERNEL->config->value(zprobe_checksum, slow_feedrate_checksum)->by_default(5)->as_number(); // feedrate in mm/sec
+    this->fast_feedrate = THEKERNEL->config->value(zprobe_checksum, fast_feedrate_checksum)->by_default(100)->as_number(); // feedrate in mm/sec
 
     // see what type of arm solution we need to use
     this->is_delta =  THEKERNEL->config->value(delta_homing_checksum)->by_default(false)->as_bool();
-
 }
 
-bool ZProbe::wait_for_probe(int steps[])
+bool ZProbe::wait_for_probe(int steps[3])
 {
     unsigned int debounce = 0;
     while(true) {
@@ -118,43 +136,206 @@ void ZProbe::on_idle(void *argument)
 }
 
 // single probe and report amount moved
-bool ZProbe::run_probe(int *steps)
+bool ZProbe::run_probe(int& steps, bool fast)
 {
     // Enable the motors
     THEKERNEL->stepper->turn_enable_pins_on();
+    this->current_feedrate = (fast ? this->fast_feedrate : this->slow_feedrate) * this->steps_per_mm[Z_AXIS]; // steps/sec
 
     // move Z down
-    this->running= true;
+    this->running = true;
     this->steppers[Z_AXIS]->set_speed(0); // will be increased by acceleration tick
-    this->steppers[Z_AXIS]->move(true, 100*this->steps_per_mm[Z_AXIS]); // always probes down, no more than 100mm
+    this->steppers[Z_AXIS]->move(true, 1000 * this->steps_per_mm[Z_AXIS]); // always probes down, no more than 1000mm TODO should be 2*maxz
     if(this->is_delta) {
         // for delta need to move all three actuators
         this->steppers[X_AXIS]->set_speed(0);
-        this->steppers[X_AXIS]->move(true, 100*this->steps_per_mm[X_AXIS]);
+        this->steppers[X_AXIS]->move(true, 1000 * this->steps_per_mm[X_AXIS]);
         this->steppers[Y_AXIS]->set_speed(0);
-        this->steppers[Y_AXIS]->move(true, 100*this->steps_per_mm[Y_AXIS]);
+        this->steppers[Y_AXIS]->move(true, 1000 * this->steps_per_mm[Y_AXIS]);
     }
 
-    bool r= wait_for_probe(steps);
-    this->running= false;
+    int s[3];
+    bool r = wait_for_probe(s);
+    steps= s[2]; // only need z
+    this->running = false;
     return r;
 }
 
+bool ZProbe::return_probe(int steps)
+{
+    // move probe back to where it was
+    this->current_feedrate = this->fast_feedrate * this->steps_per_mm[Z_AXIS]; // feedrate in steps/sec
+    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) {
+        this->steppers[X_AXIS]->set_speed(0);
+        this->steppers[X_AXIS]->move(dir, steps);
+        this->steppers[Y_AXIS]->set_speed(0);
+        this->steppers[Y_AXIS]->move(dir, steps);
+    }
+    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);
+    }
+
+    this->running = false;
+
+    return true;
+}
+
+// calculate the X and Y positions for the three towers given the radius from the center
+static std::tuple<float, float, float, float, float, float> getCoordinates(float radius)
+{
+    float px = 0.866F * radius; // ~sin(60)
+    float py = 0.5F * radius; // cos(60)
+    float t1x = -px, t1y = -py; // X Tower
+    float t2x = px, t2y = -py; // Y Tower
+    float t3x = 0.0F, t3y = radius; // Z Tower
+    return std::make_tuple(t1x, t1y, t2x, t2y, t3x, t3y);
+}
+
+bool ZProbe::probe_delta_tower(int& steps, float x, float y)
+{
+    int s;
+    // move to tower
+    coordinated_move(x, y, NAN, this->fast_feedrate);
+    if(!run_probe(s)) return false;
+
+    // return to original Z
+    return_probe(s);
+    steps= s;
+
+    return true;
+}
+
 /* Run a calibration routine for a delta
     1. Home
     2. probe for z bed
-    3. move to base of each tower at 5mm above bed
-    4. probe down to bed
-    5. probe next tower
-    6. calculate trim offsets and apply them
-    7. Home
-    8. Probe center
+    3. probe initial tower positions
+    4. set initial trims such that trims will be minimal negative values
+    5. home, probe three towers again
+    6. calculate trim offset and apply to all trims
+    7. repeat 5, 6 4 times to converge on a solution
+    8. home, Probe center
     9. calculate delta radius and apply it
     10. check level
 */
 
 bool ZProbe::calibrate_delta(Gcode *gcode)
 {
+    // zero trim values
+    set_trim(0, 0, 0, &(StreamOutput::NullStream));
+
+    // home
+    home();
+
+    // find bed, run at fast rate
+    int s;
+    if(!run_probe(s, true)) return false;
+
+    // how far to move down from home before probe
+    int probestart = s - (this->probe_height*this->steps_per_mm[Z_AXIS]);
+    gcode->stream->printf("Probe start ht is %f mm\n", probestart/this->steps_per_mm[Z_AXIS]);
+
+    // get probe points
+    float t1x, t1y, t2x, t2y, t3x, t3y;
+    std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
+
+    // move to start position
+    home();
+    return_probe(-probestart);
+
+    // get initial probes
+    // probe the base of the X tower
+    if(!probe_delta_tower(s, t1x, t1y)) return false;
+    float t1z= s / this->steps_per_mm[Z_AXIS];
+    gcode->stream->printf("T1-1 Z:%1.4f C:%d\n", t1z, s);
+
+    // probe the base of the Y tower
+    if(!probe_delta_tower(s, t2x, t2y)) return false;
+    float t2z= s / this->steps_per_mm[Z_AXIS];
+    gcode->stream->printf("T2-1 Z:%1.4f C:%d\n", t2z, s);
+
+    // probe the base of the Z tower
+    if(!probe_delta_tower(s, t3x, t3y)) return false;
+    float t3z= s / this->steps_per_mm[Z_AXIS];
+    gcode->stream->printf("T3-1 Z:%1.4f C:%d\n", t3z, s);
+
+    float trimscale= 1.2522F; // empirically determined
+
+    // set initial trims to worst case so we always have a negative trim
+    float min= std::min({t1z, t2z, t3z});
+    float trimx= (min-t1z)*trimscale, trimy= (min-t2z)*trimscale, trimz= (min-t3z)*trimscale;
+
+    // set initial trim
+    set_trim(trimx, trimy, trimz, gcode->stream);
+
+    for (int i = 1; i <= 4; ++i) {
+        // home and move probe to start position just above the bed
+        home();
+        return_probe(-probestart);
+
+        // probe the base of the X tower
+        if(!probe_delta_tower(s, t1x, t1y)) return false;
+        t1z= s / this->steps_per_mm[Z_AXIS];
+        gcode->stream->printf("T1-2-%d Z:%1.4f C:%d\n", i, t1z, s);
+
+        // probe the base of the Y tower
+        if(!probe_delta_tower(s, t2x, t2y)) return false;
+        t2z= s / this->steps_per_mm[Z_AXIS];
+        gcode->stream->printf("T2-2-%d Z:%1.4f C:%d\n", i, t2z, s);
+
+        // probe the base of the Z tower
+        if(!probe_delta_tower(s, t3x, t3y)) return false;
+        t3z= s / this->steps_per_mm[Z_AXIS];
+        gcode->stream->printf("T3-2-%d Z:%1.4f C:%d\n", i, t3z, s);
+
+        auto mm= std::minmax({t1z, t2z, t3z});
+        if((mm.second-mm.first) < 0.03F) break; // probably as good as it gets, TODO set 0.02 as config value
+
+        // set new trim values based on min difference
+        min= mm.first;
+        trimx += (min-t1z)*trimscale;
+        trimy += (min-t2z)*trimscale;
+        trimz += (min-t3z)*trimscale;
+
+        // set trim
+        set_trim(trimx, trimy, trimz, gcode->stream);
+
+        // flush the output
+        THEKERNEL->call_event(ON_IDLE);
+    }
+
+    // move probe to start position just above the bed
+    home();
+    return_probe(-probestart);
+
+    // probe the base of the three towers again to see if we are level
+    int dx= 0, dy= 0, dz= 0;
+    if(!probe_delta_tower(dx, t1x, t1y)) return false;
+    gcode->stream->printf("T1-final Z:%1.4f C:%d\n", dx / this->steps_per_mm[Z_AXIS], dx);
+    if(!probe_delta_tower(dy, t2x, t2y)) return false;
+    gcode->stream->printf("T2-final Z:%1.4f C:%d\n", dy / this->steps_per_mm[Z_AXIS], dy);
+    if(!probe_delta_tower(dz, t3x, t3y)) return false;
+    gcode->stream->printf("T3-final Z:%1.4f C:%d\n", dz / this->steps_per_mm[Z_AXIS], dz);
+
+    // compare the three and report
+    auto mm= std::minmax({dx, dy, dz});
+    gcode->stream->printf("max endstop delta= %f\n", (mm.second-mm.first)/this->steps_per_mm[Z_AXIS]);
+
+    // probe center
+    int dc;
+    if(!probe_delta_tower(dc, 0, 0)) return false;
+
+    // TODO adjust delta radius
+    gcode->stream->printf("Center Z:%1.4f C:%d\n", dc / this->steps_per_mm[Z_AXIS], dc);
+
+    mm= std::minmax({dx, dy, dz, dc});
+    gcode->stream->printf("max delta= %f\n", (mm.second-mm.first)/this->steps_per_mm[Z_AXIS]);
 
     return true;
 }
@@ -162,64 +343,52 @@ bool ZProbe::calibrate_delta(Gcode *gcode)
 void ZProbe::on_gcode_received(void *argument)
 {
     Gcode *gcode = static_cast<Gcode *>(argument);
-    Robot *robot = THEKERNEL->robot;
 
     if( gcode->has_g) {
         // G code processing
-        if( gcode->g == 30 ) {
+        if( gcode->g == 30 ) { // simple Z probe
             gcode->mark_as_taken();
             // first wait for an empty queue i.e. no moves left
             THEKERNEL->conveyor->wait_for_empty_queue();
 
-            if( gcode->has_letter('F') ) {
-                // probe speed
-                this->feedrate = robot->to_millimeters( gcode->get_value('F') ) * this->steps_per_mm[Z_AXIS] / robot->seconds_per_minute;
-            }
-            int steps[3];
-            if(run_probe(steps)){
-                gcode->stream->printf("Z:%1.4f C:%d\n", steps[2]/this->steps_per_mm[Z_AXIS], steps[Z_AXIS]);
+            int steps;
+            if(run_probe(steps)) {
+                gcode->stream->printf("Z:%1.4f C:%d\n", steps / this->steps_per_mm[Z_AXIS], steps);
                 // move back to where it started, unless a Z is specified
                 if(gcode->has_letter('Z')) {
                     // set Z to the specified value, and leave probe where it is
                     THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
-                }else{
-                    // move probe back to where it was
-                    this->running= true;
-                    this->steppers[Z_AXIS]->set_speed(0); // will be increased by acceleration tick
-                    this->steppers[Z_AXIS]->move(false, steps[Z_AXIS]);
-                    if(this->is_delta) {
-                        this->steppers[X_AXIS]->set_speed(0);
-                        this->steppers[X_AXIS]->move(false, steps[X_AXIS]);
-                        this->steppers[Y_AXIS]->set_speed(0);
-                        this->steppers[Y_AXIS]->move(false, steps[Y_AXIS]);
-                    }
-                    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);
-                    }
-
-                    this->running= false;
+                } else {
+                    return_probe(steps);
                 }
-            }else{
+            } else {
                 gcode->stream->printf("ZProbe not triggered\n");
             }
 
-        }else if( gcode->g == 32 ) {
+        } else if( gcode->g == 32 ) { // auto calibration for delta, Z bed mapping for cartesian
+            // first wait for an empty queue i.e. no moves left
+            THEKERNEL->conveyor->wait_for_empty_queue();
             gcode->mark_as_taken();
             if(is_delta) {
-                calibrate_delta(gcode);
-            }else{
-                 gcode->stream->printf("Not supported yet\n");
+                if(calibrate_delta(gcode)) {
+                    gcode->stream->printf("Calibration complete, save settings with M500\n");
+                } else {
+                    gcode->stream->printf("Calibration failed to complete, probe not triggered\n");
+                }
+            } else {
+                // TODO create Z height map for bed
+                gcode->stream->printf("Not supported yet\n");
             }
         }
 
     } else if(gcode->has_m) {
         // M code processing here
         if(gcode->m == 119) {
-            int c= this->pin.get();
+            int c = this->pin.get();
             gcode->stream->printf(" Probe: %d", c);
             gcode->add_nl = true;
             gcode->mark_as_taken();
+
         }
     }
 }
@@ -235,13 +404,15 @@ uint32_t ZProbe::acceleration_tick(uint32_t dummy)
         if( !this->steppers[c]->moving ) continue;
 
         uint32_t current_rate = this->steppers[c]->steps_per_second;
-        uint32_t target_rate = int(floor(this->feedrate));
+        uint32_t target_rate = int(floor(this->current_feedrate));
 
-        if( current_rate < target_rate ){
-            uint32_t rate_increase = int(floor((THEKERNEL->planner->acceleration/THEKERNEL->stepper->acceleration_ticks_per_second)*this->steps_per_mm[c]));
+        if( current_rate < target_rate ) {
+            uint32_t rate_increase = int(floor((THEKERNEL->planner->acceleration / THEKERNEL->stepper->acceleration_ticks_per_second) * this->steps_per_mm[c]));
             current_rate = min( target_rate, current_rate + rate_increase );
         }
-        if( current_rate > target_rate ){ current_rate = target_rate; }
+        if( current_rate > target_rate ) {
+            current_rate = target_rate;
+        }
 
         // steps per second
         this->steppers[c]->set_speed(max(current_rate, THEKERNEL->stepper->minimum_steps_per_second));
@@ -249,3 +420,46 @@ uint32_t ZProbe::acceleration_tick(uint32_t dummy)
 
     return 0;
 }
+
+// issue a coordinated move directly to robot, and return when done
+// Only move the coordinates that are passed in as not nan
+void ZProbe::coordinated_move(float x, float y, float z, float feedrate)
+{
+    char buf[32];
+    char cmd[64] = "G0";
+    if(!isnan(x)) {
+        int n = snprintf(buf, sizeof(buf), " X%f", x);
+        strncat(cmd, buf, n);
+    }
+    if(!isnan(y)) {
+        int n = snprintf(buf, sizeof(buf), " Y%f", y);
+        strncat(cmd, buf, n);
+    }
+    if(!isnan(z)) {
+        int n = snprintf(buf, sizeof(buf), " Z%f", z);
+        strncat(cmd, buf, n);
+    }
+
+    // use specified feedrate (mm/sec)
+    int n = snprintf(buf, sizeof(buf), " F%f", feedrate * 60); // feed rate is converted to mm/min
+    strncat(cmd, buf, n);
+
+    Gcode gc(cmd, &(StreamOutput::NullStream));
+    THEKERNEL->robot->on_gcode_received(&gc);
+    THEKERNEL->conveyor->wait_for_empty_queue();
+}
+
+// issue home command
+void ZProbe::home()
+{
+    Gcode gc("G28", &(StreamOutput::NullStream));
+    THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
+}
+
+void 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);
+}

diff --git a/src/modules/tools/zprobe/ZProbe.h b/src/modules/tools/zprobe/ZProbe.h
index 51397e7..52dd853 100644 (file)
--- a/src/modules/tools/zprobe/ZProbe.h
+++ b/src/modules/tools/zprobe/ZProbe.h
@@ -13,6 +13,7 @@
 
 class StepperMotor;
 class Gcode;
+class StreamOutput;
 
 class ZProbe: public Module
 {
@@ -26,11 +27,21 @@ public:
 
 
 private:
-    bool wait_for_probe(int distance[]);
-    bool run_probe(int *steps);
+    bool wait_for_probe(int steps[3]);
+    bool run_probe(int& steps, bool fast= false);
+    bool probe_delta_tower(int& steps, float x, float y);
+    bool return_probe(int steps);
     bool calibrate_delta(Gcode *gcode);
-
-    float          feedrate;
+    void coordinated_move(float x, float y, float z, float feedrate);
+    void home();
+    void set_trim(float x, float y, float z, StreamOutput *stream);
+
+    float          endstop_radius;
+    float          probe_radius;
+    float          probe_height;
+    float          current_feedrate;
+    float          slow_feedrate;
+    float          fast_feedrate;
     float          steps_per_mm[3];
     unsigned int   mcode;
     bool           enabled;

diff --git a/src/modules/utils/player/Player.cpp b/src/modules/utils/player/Player.cpp
index 462d7f6..bc87cf4 100644 (file)
--- a/src/modules/utils/player/Player.cpp
+++ b/src/modules/utils/player/Player.cpp
@@ -323,7 +323,7 @@ void Player::on_main_loop(void *argument)
                 this->current_stream->printf("%s", buf);
                 struct SerialMessage message;
                 message.message = buf;
-                message.stream = &(StreamOutput::NullStream); // we don't really need to see the ok
+                message.stream = this->current_stream;
 
                 // waits for the queue to have enough room
                 THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message);