WIP

Grbl_Esp32/Custom/parallel_delta.cpp

@@ -24,10 +24,10 @@
   The arm 0 values (angle) are the arms at horizontal.
   Positive angles are below horizontal.
   The machine's Z zero point in the kinematics is parallel to the arm axes.
-  Delta_z_offset is the offset to the end effector joints at arm angle zero.
+  Delta_z_offset is the Z distance from the arm axes to the end effector joints 
-  The is calculated at startup and used in the forward kinematics
+  at arm angle zero. The is calculated at startup and used in the forward kinematics
 
-  Feedrate in gcode is in the cartesian uints. This must be converted to the
+  Feedrate in gcode is in the cartesian units. This must be converted to the
   angles. This is done by calculating the segment move distance and the angle 
   move distance and applying that ration to the feedrate. 
 
@@ -87,9 +87,10 @@ void machine_init() {
     float cartesian[N_AXIS] = { 0.0, 0.0, 0.0 };
 
     calc_forward_kinematics(angles, cartesian);  // Sets the cartesian values
+    delta_z_offset = cartesian[Z_AXIS];
 
     // print a startup message to show the kinematics are enables
-    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]);
+    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, delta_z_offset);
 }
 
 bool user_defined_homing() {  // true = do not continue with normal Grbl homing
@@ -110,9 +111,32 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
     float          feed_rate            = pl_data->feed_rate;  // save original feed rate
     bool           start_position_erorr = false;
     bool           show_error           = true;  // shows error once
+    float          pos_cart[N_AXIS];
+    KinematicError status;
+
+    // convert the current position to cartesian
+
+    calc_forward_kinematics(position, pos_cart);
+
+    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "From Angs:  %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
+    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "From Cart:  %3.3f %3.3f %3.3f", pos_cart[0], pos_cart[1], pos_cart[2]);
+
+    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Raw Target: %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
+    grbl_msg_sendf(CLIENT_SERIAL,
+                   MsgLevel::Info,
+                   "Offsets: %3.3f %3.3f %3.3f",
+                   delta_z_offset,
+                   gc_state.coord_offset[Z_AXIS],
+                   gc_state.coord_system[Z_AXIS]);
+
+    //target[Z_AXIS] += delta_z_offset + (gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS]);
+
+    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "To: %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
+    status = delta_calcInverse(target, motor_angles);
+    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Angles: %3.3f %3.3f %3.3f", motor_angles[0], motor_angles[1], motor_angles[2]);
 
     // see if start is in work area...if not skip segments and try to go to target
-    KinematicError status = delta_calcInverse(position, motor_angles);
+    status = delta_calcInverse(position, motor_angles);
 
     if (status == KinematicError::OUT_OF_RANGE) {
         grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error");
@@ -127,6 +151,14 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
         return;
     }
 
+    target[Z_AXIS] -= delta_z_offset;  // restore it
+    memcpy(position, target, sizeof(target));
+
+    memcpy(last_angle, motor_angles, sizeof(motor_angles));
+
+    mc_line(motor_angles, pl_data);
+    return;
+
     position[X_AXIS] += gc_state.coord_offset[X_AXIS];
     position[Y_AXIS] += gc_state.coord_offset[Y_AXIS];
     position[Z_AXIS] += gc_state.coord_offset[Z_AXIS];
@@ -324,6 +356,18 @@ void kinematics_post_homing() {
 #ifdef USE_CUSTOM_HOMING
 
 #else
+    //sys_position[Z_AXIS] = delta_z_offset * axis_settings[Z_AXIS]->steps_per_mm->get();
+    //plan_sync_position();
+    last_angle[X_AXIS] = sys_position[X_AXIS] / axis_settings[X_AXIS]->steps_per_mm->get();
+    last_angle[Y_AXIS] = sys_position[Y_AXIS] / axis_settings[Y_AXIS]->steps_per_mm->get();
+    last_angle[Z_AXIS] = sys_position[Z_AXIS] / axis_settings[Z_AXIS]->steps_per_mm->get();
+
+    grbl_msg_sendf(CLIENT_SERIAL,
+                   MsgLevel::Info,
+                   "kinematics_post_homing: %3.3f, %3.3f, %3.3f",
+                   last_angle[X_AXIS],
+                   last_angle[Y_AXIS],
+                   last_angle[Z_AXIS]);
 
 #endif
 }

Grbl_Esp32/src/Config.h

@@ -96,11 +96,11 @@ const int MAX_N_AXIS = 6;
 //#define CONNECT_TO_SSID  "your SSID"
 //#define SSID_PASSWORD  "your SSID password"
 //CONFIGURE_EYECATCH_BEGIN (DO NOT MODIFY THIS LINE)
-#define ENABLE_BLUETOOTH  // enable bluetooth
+//#define ENABLE_BLUETOOTH  // enable bluetooth
 
 #define ENABLE_SD_CARD  // enable use of SD Card to run jobs
 
-#define ENABLE_WIFI  //enable wifi
+//#define ENABLE_WIFI  //enable wifi
 
 #if defined(ENABLE_WIFI) || defined(ENABLE_BLUETOOTH)
 #    define WIFI_OR_BLUETOOTH

Grbl_Esp32/src/Machines/tapster_pro.h

@@ -1,4 +1,5 @@
 #pragma once
+// clang-format off
 
 /*
     tapster_pro.h
@@ -37,9 +38,10 @@
 
 #define HOMING_CURRENT_REDUCTION 1.0
 
-
 */
 
+#define N_AXIS 3
+
 #define USE_KINEMATICS      // there are kinematic equations for this machine
 #define USE_FWD_KINEMATICS  // report in cartesian
 #define USE_MACHINE_INIT    // There is some custom initialization for this machine
@@ -51,22 +53,11 @@
 #define LENGTH_FIXED_SIDE   294.449f  // sized of fixed side triangel
 #define LENGTH_EFF_SIDE     86.6025f    // size of end effector side triangle
 #define SEGMENT_LENGTH      0.5f         // segment length in mm
-#define MAX_NEGATIVE_ANGLE -45.0f   //
+#define MAX_NEGATIVE_ANGLE  -0.75f   //
 
 // ================== Config ======================
 
-#ifdef HOMING_CYCLE_0
+// Set $Homing/Cycle0=XYZ
-#    undef HOMING_CYCLE_0
-#endif
-#define HOMING_CYCLE_0 ((1 << X_AXIS) | (1 << Y_AXIS) | (1 << Z_AXIS))
-
-#ifdef HOMING_CYCLE_1
-#    undef HOMING_CYCLE_1
-#endif
-
-#ifdef HOMING_CYCLE_2
-#    undef HOMING_CYCLE_2
-#endif
 
 // ================== CPU MAP ======================
 
@@ -119,33 +110,34 @@
 #define DEFAULT_Y_STEPS_PER_MM  DEFAULT_X_STEPS_PER_MM
 #define DEFAULT_Z_STEPS_PER_MM  DEFAULT_X_STEPS_PER_MM
 
-#define DEFAULT_X_MAX_RATE 400.0  // mm/min
+#define DEFAULT_X_MAX_RATE      100.0  // mm/min
 #define DEFAULT_Y_MAX_RATE      DEFAULT_X_MAX_RATE
 #define DEFAULT_Z_MAX_RATE      DEFAULT_X_MAX_RATE
 
-#define DEFAULT_X_ACCELERATION 90.0
+#define DEFAULT_X_ACCELERATION  20.0
 #define DEFAULT_Y_ACCELERATION  DEFAULT_X_ACCELERATION
 #define DEFAULT_Z_ACCELERATION  DEFAULT_X_ACCELERATION
 
 //  homing
 #define DEFAULT_HOMING_FEED_RATE    25
-#define DEFAULT_HOMING_SEEK_RATE 50
+#define DEFAULT_HOMING_SEEK_RATE    100
-#define DEFAULT_HOMING_DIR_MASK 7
+#define DEFAULT_HOMING_DIR_MASK     (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS))  // all axes home negative
 #define DEFAULT_HOMING_ENABLE       1
 #define DEFAULT_INVERT_LIMIT_PINS   0
 
-// homing
+// The machine homes up and above center. MPos is the axis angle in radians
+// at the homing posiiton
 
-#define DEFAULT_USER_INT_80 800  // $80 User integer setting
+#define DEFAULT_X_HOMING_MPOS   -0.75  // neagtive because above horizontal
-#define DEFAULT_USER_INT_81 350  // $80 User integer setting
+#define DEFAULT_Y_HOMING_MPOS   -0.75
+#define DEFAULT_Z_HOMING_MPOS   -0.75
 
-#define DEFAULT_X_MAX_TRAVEL 1.75  // 100 dgrees in radians
+// the total travel is straight down from horizontal (pi/2) + the up travel
+#define DEFAULT_X_MAX_TRAVEL    ((M_PI / 2.0) - DEFAULT_X_HOMING_MPOS)
 #define DEFAULT_Y_MAX_TRAVEL    DEFAULT_X_MAX_TRAVEL
 #define DEFAULT_Z_MAX_TRAVEL    DEFAULT_X_MAX_TRAVEL
 
-#define DEFAULT_X_HOMING_MPOS 0.75
+#define DEFAULT_HOMING_PULLOFF  -DEFAULT_X_HOMING_MPOS
-#define DEFAULT_Y_HOMING_MPOS 0.75
-#define DEFAULT_Z_HOMING_MPOS 0.75
 
 #define DEFAULT_X_CURRENT 1.0
 #define DEFAULT_Y_CURRENT DEFAULT_X_CURRENT

Grbl_Esp32/src/Machines/test_drive.h

@@ -37,6 +37,8 @@
 
 #define MACHINE_NAME "Test Drive - Demo Only No I/O!"
 
+#define N_AXIS 3
+
 #define SPINDLE_TYPE    SpindleType::NONE
 
 #ifdef USE_RMT_STEPS

Grbl_Esp32/src/Motors/TrinamicDriver.cpp

@@ -66,6 +66,7 @@ namespace Motors {
             tmcstepper->setSPISpeed(TRINAMIC_SPI_FREQ);
         }
 
+        read_settings();  // pull info from settings
         config_message();
 
         // init() must be called later, after all TMC drivers have CS pins setup.
@@ -163,6 +164,17 @@ namespace Motors {
         if (has_errors)
             return;
 
+        float max_travel = axis_settings[_axis_index]->max_travel->get();
+        float mpos       = axis_settings[_axis_index]->home_mpos->get();
+
+        if (bit_istrue(homing_dir_mask->get(), bit(_axis_index))) {
+            _position_min = mpos;
+            _position_max = mpos + max_travel;
+        } else {
+            _position_min = mpos - max_travel;
+            _position_max = mpos;
+        }
+
         uint16_t run_i_ma = (uint16_t)(axis_settings[_axis_index]->run_current->get() * 1000.0);
         float    hold_i_percent;