Merge Dxl test (#596)

* Changes, not ready yet

* Some updates for M67

* WIP

* Updates

* Code cleanup and speed tweaks

* Prep for PR

* Cleaned up user I/O to use UNDEFINED_PIN as defaults.

* Cleaned up spurious code.

Grbl_Esp32/Custom/custom_code_template.cpp

@@ -108,7 +108,7 @@ bool kinematics_pre_homing(uint8_t cycle_mask))
 void kinematics_post_homing() {}
 #endif
 
-#ifdef USE_FWD_KINEMATIC
+#ifdef USE_FWD_KINEMATICS
 /*
   The status command uses forward_kinematics() to convert
   your motor positions to cartesian X,Y,Z... coordinates.

Grbl_Esp32/Custom/esp32_printer_controller.cpp

@@ -134,7 +134,7 @@ bool kinematics_pre_homing(uint8_t cycle_mask))
 void kinematics_post_homing() {}
 #endif
 
-#ifdef USE_FWD_KINEMATIC
+#ifdef USE_FWD_KINEMATICS
 /*
   The status command uses forward_kinematics() to convert
   your motor positions to cartesian X,Y,Z... coordinates.

Grbl_Esp32/Custom/parallel_delta.cpp

@@ -0,0 +1,312 @@
+
+/*
+  parallel_delta.cpp - 
+  
+  Copyright (c) 2019    Barton Dring @buildlog, 
+                        Jason Huggins, Tapster Robotics
+
+  Kinematics for a parallel delta robot.
+      
+  ==================== How it Works ====================================
+
+  On a delta machine, Grbl axis units are in radians
+  The kinematics converts the cartesian moves in gcode into
+  the radians to move the arms. The Grbl motion planner never sees
+  the actual cartesian values.
+
+  To make the moves straight and smooth on a delta, the cartesian moves
+  are broken into small segments where the non linearity will not be noticed.
+  This is similar to how Grgl draws arcs.
+
+  If you request MPos status it will tell you the position in
+  arm angles. The MPos will report in cartesian values using forward kinematics. 
+
+  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.
+  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
+  angles. This is done by calculating the segment move distance and the angle 
+  move distance and applying that ration to the feedrate. 
+
+  TODO Cleanup
+  Update so extra axes get delt with ... passed through properly
+
+  ============================================================================
+
+  Grbl is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+  Grbl is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+  You should have received a copy of the GNU General Public License
+  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+    FYI: http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/
+    Better: http://hypertriangle.com/~alex/delta-robot-tutorial/
+*/
+
+enum class KinematicError : uint8_t {
+    NONE               = 0,
+    OUT_OF_RANGE       = 1,
+    ANGLE_TOO_NEGATIVE = 2,
+};
+
+// trigonometric constants to speed calculations
+const float sqrt3  = 1.732050807;
+const float dtr    = M_PI / (float)180.0;  // degrees to radians
+const float sin120 = sqrt3 / 2.0;
+const float cos120 = -0.5;
+const float tan60  = sqrt3;
+const float sin30  = 0.5;
+const float tan30  = 1.0 / sqrt3;
+
+// the geometry of the delta
+const float rf = RADIUS_FIXED;       // radius of the fixed side (length of motor cranks)
+const float re = RADIUS_EFF;         // radius of end effector side (length of linkages)
+const float f  = LENGTH_FIXED_SIDE;  // sized of fixed side triangel
+const float e  = LENGTH_EFF_SIDE;    // size of end effector side triangle
+
+static float last_angle[N_AXIS] = { 0.0, 0.0, 0.0 };  // A place to save the previous motor angles for distance/feed rate calcs
+float        delta_z_offset;                          // Z offset of the effector from the arm centers
+
+// prototypes for helper functions
+int            calc_forward_kinematics(float* angles, float* cartesian);
+KinematicError delta_calcInverse(float* cartesian, float* angles);
+KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta);
+float          three_axis_dist(float* point1, float* point2);
+
+void machine_init() {
+    // Calculate the Z offset at the motor zero angles ...
+    // Z offset is the z distance from the motor axes to the end effector axes at zero angle
+    float angles[N_AXIS]    = { 0.0, 0.0, 0.0 };
+    float cartesian[N_AXIS] = { 0.0, 0.0, 0.0 };
+
+    calc_forward_kinematics(angles, cartesian);  // Sets the cartesian values
+
+    // print a startup message to show the kinematics are enables
+    grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delata Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]);
+}
+
+bool user_defined_homing() {  // true = do not continue with normal Grbl homing
+    return true;
+}
+
+void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position)  //The target and position are provided in MPos
+{
+    float dx, dy, dz;  // distances in each cartesian axis
+    float motor_angles[N_AXIS];
+
+    float seg_target[N_AXIS];                         // The target of the current segment
+    float feed_rate            = pl_data->feed_rate;  // save original feed rate
+    bool  start_position_erorr = false;
+    bool  show_error           = true;  // shows error once
+
+    // see if start is in work area...if not skip segments and try to go to target
+    KinematicError status = delta_calcInverse(position, motor_angles);
+
+    if (status == KinematicError::OUT_OF_RANGE) {
+        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error");
+        start_position_erorr = true;
+    }
+
+    // Check the destination to see if it is in work area
+    status = delta_calcInverse(target, motor_angles);
+
+    if (status == KinematicError::OUT_OF_RANGE) {
+        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable");
+        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];
+
+    // calculate cartesian move distance for each axis
+
+    dx         = target[X_AXIS] - position[X_AXIS];
+    dy         = target[Y_AXIS] - position[Y_AXIS];
+    dz         = target[Z_AXIS] - position[Z_AXIS];
+    float dist = sqrt((dx * dx) + (dy * dy) + (dz * dz));
+
+    // determine the number of segments we need	... round up so there is at least 1 (except when dist is 0)
+    uint32_t segment_count = ceil(dist / SEGMENT_LENGTH);
+
+    float segment_dist = dist / ((float)segment_count);  // distance of each segment...will be used for feedrate conversion
+
+    for (uint32_t segment = 1; segment <= segment_count; segment++) {
+        // determine this segment's target
+        seg_target[X_AXIS] = position[X_AXIS] + (dx / float(segment_count) * segment);
+        seg_target[Y_AXIS] = position[Y_AXIS] + (dy / float(segment_count) * segment);
+        seg_target[Z_AXIS] = position[Z_AXIS] + (dz / float(segment_count) * segment);
+
+        // calculate the delta motor angles
+        KinematicError status = delta_calcInverse(seg_target, motor_angles);
+
+        if (status == KinematicError ::NONE) {
+            float delta_distance = three_axis_dist(motor_angles, last_angle);
+
+            // save angles for next distance calc
+            memcpy(last_angle, motor_angles, sizeof(motor_angles));
+
+            if (pl_data->motion.rapidMotion) {
+                pl_data->feed_rate = feed_rate;
+            } else {
+                pl_data->feed_rate = (feed_rate * delta_distance / segment_dist);
+            }
+
+            mc_line(motor_angles, pl_data);
+
+        } else {
+            if (show_error) {
+                grbl_msg_sendf(CLIENT_SERIAL,
+                               MsgLevel::Info,
+                               "Error:%d, Angs X:%4.3f Y:%4.3f Z:%4.3f]\r\n\r\n",
+                               status,
+                               motor_angles[0],
+                               motor_angles[1],
+                               motor_angles[2]);
+                show_error = false;
+            }
+        }
+    }
+}
+
+// inverse kinematics: cartesian -> angles
+// returned status: 0=OK, -1=non-existing position
+KinematicError delta_calcInverse(float* cartesian, float* angles) {
+    angles[0] = angles[1] = angles[2] = 0;
+    KinematicError status             = KinematicError::NONE;
+
+    status = delta_calcAngleYZ(cartesian[X_AXIS], cartesian[Y_AXIS], cartesian[Z_AXIS], angles[0]);
+    if (status != KinematicError ::NONE) {
+        return status;
+    }
+
+    status = delta_calcAngleYZ(cartesian[X_AXIS] * cos120 + cartesian[Y_AXIS] * sin120,
+                               cartesian[Y_AXIS] * cos120 - cartesian[X_AXIS] * sin120,
+                               cartesian[Z_AXIS],
+                               angles[1]);  // rotate coords to +120 deg
+    if (status != KinematicError ::NONE) {
+        return status;
+    }
+
+    status = delta_calcAngleYZ(cartesian[X_AXIS] * cos120 - cartesian[Y_AXIS] * sin120,
+                               cartesian[Y_AXIS] * cos120 + cartesian[X_AXIS] * sin120,
+                               cartesian[Z_AXIS],
+                               angles[2]);  // rotate coords to -120 deg
+    if (status != KinematicError ::NONE) {
+        return status;
+    }
+
+    //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "xyx (%4.3f,%4.3f,%4.3f) ang (%4.3f,%4.3f,%4.3f)", x0, y0, z0, theta1, theta2, theta3);
+    return status;
+}
+
+// inverse kinematics: angles -> cartesian
+int calc_forward_kinematics(float* angles, float* catesian) {
+    float t = (f - e) * tan30 / 2;
+
+    float y1 = -(t + rf * cos(angles[0]));
+    float z1 = -rf * sin(angles[0]);
+
+    float y2 = (t + rf * cos(angles[1])) * sin30;
+    float x2 = y2 * tan60;
+    float z2 = -rf * sin(angles[1]);
+
+    float y3 = (t + rf * cos(angles[2])) * sin30;
+    float x3 = -y3 * tan60;
+    float z3 = -rf * sin(angles[2]);
+
+    float dnm = (y2 - y1) * x3 - (y3 - y1) * x2;
+
+    float w1 = y1 * y1 + z1 * z1;
+    float w2 = x2 * x2 + y2 * y2 + z2 * z2;
+    float w3 = x3 * x3 + y3 * y3 + z3 * z3;
+
+    // x = (a1*z + b1)/dnm
+    float a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1);
+    float b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0;
+
+    // y = (a2*z + b2)/dnm;
+    float a2 = -(z2 - z1) * x3 + (z3 - z1) * x2;
+    float b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0;
+
+    // a*z^2 + b*z + c = 0
+    float a = a1 * a1 + a2 * a2 + dnm * dnm;
+    float b = 2 * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm);
+    float c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - re * re);
+
+    // discriminant
+    float d = b * b - (float)4.0 * a * c;
+    if (d < 0)
+        return -1;  // non-existing point
+
+    catesian[Z_AXIS] = -(float)0.5 * (b + sqrt(d)) / a;
+    catesian[X_AXIS] = (a1 * catesian[Z_AXIS] + b1) / dnm;
+    catesian[Y_AXIS] = (a2 * catesian[Z_AXIS] + b2) / dnm;
+    return 0;
+}
+// helper functions, calculates angle theta1 (for YZ-pane)
+KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta) {
+    float y1 = -0.5 * 0.57735 * f;  // f/2 * tg 30
+    y0 -= 0.5 * 0.57735 * e;        // shift center to edge
+    // z = a + b*y
+    float a = (x0 * x0 + y0 * y0 + z0 * z0 + rf * rf - re * re - y1 * y1) / (2 * z0);
+    float b = (y1 - y0) / z0;
+    // discriminant
+    float d = -(a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf);
+    if (d < 0)
+        return KinematicError::OUT_OF_RANGE;          // non-existing point
+    float yj = (y1 - a * b - sqrt(d)) / (b * b + 1);  // choosing outer point
+    float zj = a + b * yj;
+    //theta    = 180.0 * atan(-zj / (y1 - yj)) / M_PI + ((yj > y1) ? 180.0 : 0.0);
+    theta = atan(-zj / (y1 - yj)) + ((yj > y1) ? M_PI : 0.0);
+
+    if (theta < MAX_NEGATIVE_ANGLE) {
+        return KinematicError::ANGLE_TOO_NEGATIVE;
+    }
+
+    return KinematicError::NONE;
+}
+
+// Determine the unit distance between (2) 3D points
+float three_axis_dist(float* point1, float* point2) {
+    return sqrt(((point1[0] - point2[0]) * (point1[0] - point2[0])) + ((point1[1] - point2[1]) * (point1[1] - point2[1])) +
+                ((point1[2] - point2[2]) * (point1[2] - point2[2])));
+}
+// called by reporting for WPos status
+void forward_kinematics(float* position) {
+    float calc_fwd[N_AXIS];
+    int   status;
+
+    // convert the system position in steps to radians
+    float   position_radians[N_AXIS];
+    int32_t position_steps[N_AXIS];  // Copy current state of the system position variable
+    memcpy(position_steps, sys_position, sizeof(sys_position));
+    system_convert_array_steps_to_mpos(position_radians, position_steps);
+
+    // detmine the position of the end effector joint center.
+    status = calc_forward_kinematics(position_radians, calc_fwd);
+
+    if (status == 0) {
+        // apply offsets and set the returned values
+        position[X_AXIS] = calc_fwd[X_AXIS] - gc_state.coord_system[X_AXIS] + gc_state.coord_offset[X_AXIS];
+        position[Y_AXIS] = calc_fwd[Y_AXIS] - gc_state.coord_system[Y_AXIS] + gc_state.coord_offset[Y_AXIS];
+        position[Z_AXIS] = calc_fwd[Z_AXIS] - gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS];
+    } else {
+        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "MSG:Fwd Kin Error");
+    }
+}
+
+bool kinematics_pre_homing(uint8_t cycle_mask) {
+    return true;
+}
+
+void kinematics_post_homing() {}
+
+void user_m30() {}

Grbl_Esp32/src/Defaults.h

@@ -539,6 +539,42 @@
 #    define DYNAMIXEL_RTS UNDEFINED_PIN
 #endif
 
+// ================ User Digital I/O ==============================
+
+#ifndef USER_DIGITAL_PIN_0
+#    define USER_DIGITAL_PIN_0 UNDEFINED_PIN
+#endif
+
+#ifndef USER_DIGITAL_PIN_1
+#    define USER_DIGITAL_PIN_1 UNDEFINED_PIN
+#endif
+
+#ifndef USER_DIGITAL_PIN_2
+#    define USER_DIGITAL_PIN_2 UNDEFINED_PIN
+#endif
+
+#ifndef USER_DIGITAL_PIN_3
+#    define USER_DIGITAL_PIN_3 UNDEFINED_PIN
+#endif
+
+// ================ User Analog I/O ==============================
+
+#ifndef USER_ANALOG_PIN_0
+#    define USER_ANALOG_PIN_0 UNDEFINED_PIN
+#endif
+
+#ifndef USER_ANALOG_PIN_1
+#    define USER_ANALOG_PIN_1 UNDEFINED_PIN
+#endif
+
+#ifndef USER_ANALOG_PIN_2
+#    define USER_ANALOG_PIN_2 UNDEFINED_PIN
+#endif
+
+#ifndef USER_ANALOG_PIN_3
+#    define USER_ANALOG_PIN_3 UNDEFINED_PIN
+#endif
+
 #ifndef USER_ANALOG_PIN_0_FREQ
 #    define USER_ANALOG_PIN_0_FREQ 5000
 #endif

Grbl_Esp32/src/Grbl.h

@@ -106,7 +106,7 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
 bool kinematics_pre_homing(uint8_t cycle_mask);
 void kinematics_post_homing();
 
-// Called if USE_FWD_KINEMATIC is defined
+// Called if USE_FWD_KINEMATICS is defined
 void forward_kinematics(float* position);
 
 // Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined

Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h

@@ -24,9 +24,7 @@
 */
 #define MACHINE_NAME            "6 Pack Controller StepStick XYZ"
 
-#ifdef N_AXIS
-        #undef N_AXIS
-#endif
+#define N_AXIS 3
 
 #ifdef ENABLE_SD_CARD
     #undef ENABLE_SD_CARD
@@ -60,9 +58,6 @@
 #define X_STEPPER_MS3           I2SO(3)   // X_CS
 #define Y_STEPPER_MS3           I2SO(6)   // Y_CS
 #define Z_STEPPER_MS3           I2SO(11)  // Z_CS
-#define A_STEPPER_MS3           I2SO(14)  // A_CS
-#define B_STEPPER_MS3           I2SO(19)  // B_CS
-#define C_STEPPER_MS3           I2SO(22)  // C_CS
 
 #define STEPPER_RESET           GPIO_NUM_19
 

Grbl_Esp32/src/Machines/dxl_delta_module.h

@@ -2,7 +2,7 @@
 // clang-format off
 
 /*
-    dxl_delta.h
+    dxl_delta_module.h
 
     2020    - Bart Dring
 
@@ -19,9 +19,16 @@
 */
 #define MACHINE_NAME            "Dynamixel Delta"
 
+// In Socket #3
+#define DYNAMIXEL_RXD           GPIO_NUM_26
+#define DYNAMIXEL_RTS           GPIO_NUM_4
+#define DYNAMIXEL_TXD           GPIO_NUM_16
+
+/*
 #define DYNAMIXEL_TXD           GPIO_NUM_4
 #define DYNAMIXEL_RXD           GPIO_NUM_13
 #define DYNAMIXEL_RTS           GPIO_NUM_17
+*/
 
 #define SERVO_TIMER_INTerval    100   // milliseconds
 
@@ -30,7 +37,7 @@
 #define Z_DYNAMIXEL_ID          3 // protocol ID
 
 // limit servo to 180 degree motion
-#define DXL_COUNT_MIN           1024      
-#define DXL_COUNT_MAX           3072
+#define DXL_COUNT_MIN 1024      
+#define DXL_COUNT_MAX 3072
 
-#define SPINDLE_TYPE            SpindleType::NONE
+#define SPINDLE_TYPE    SpindleType::PWM

Grbl_Esp32/src/Machines/esp32_printer_controller.h

@@ -245,10 +245,10 @@
 // so non-Cartesian machines can be implemented.
 // #define USE_KINEMATICS
 
-// USE_FWD_KINEMATIC enables the forward_kinematics() function
+// USE_FWD_KINEMATICS enables the forward_kinematics() function
 // that converts motor positions in non-Cartesian coordinate
 // systems back to Cartesian form, for status reports.
-//#define USE_FWD_KINEMATIC
+//#define USE_FWD_KINEMATICS
 
 // USE_TOOL_CHANGE enables the user_tool_change() function
 // that implements custom tool change procedures.

Grbl_Esp32/src/Machines/polar_coaster.h

@@ -38,7 +38,7 @@
 
 #define SEGMENT_LENGTH 0.5 // segment length in mm
 #define USE_KINEMATICS
-#define USE_FWD_KINEMATIC // report in cartesian
+#define USE_FWD_KINEMATICS // report in cartesian
 #define USE_M30
 
 #define X_STEP_PIN              GPIO_NUM_15

Grbl_Esp32/src/Machines/tapster3.h

@@ -0,0 +1,111 @@
+#pragma once
+// clang-format off
+
+/*
+    tapster3.h
+
+    2019    - Bart Dring, Jason Huggins (Tapster Robotics)
+
+    Grbl_ESP32 is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+    Grbl is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+    You should have received a copy of the GNU General Public License
+    along with Grbl_ESP32.  If not, see <http://www.gnu.org/licenses/>.
+*/
+#define MACHINE_NAME            "Tapster 3 Delta (Dynamixel)"
+
+#define CUSTOM_CODE_FILENAME "Custom/parallel_delta.cpp"
+
+#define N_AXIS 3
+
+// ================= Firmware Customization ===================
+
+#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	
+	
+// ================== Delta Geometry ===========================
+
+#define RADIUS_FIXED    70.0;   // radius of the fixed side (length of motor cranks)
+#define RADIUS_EFF      133.5;  // radius of end effector side (length of linkages)
+#define LENGTH_FIXED_SIDE   179.437f;  // sized of fixed side triangel
+#define LENGTH_EFF_SIDE     86.6025f;  // size of end effector side triangle
+#define SEGMENT_LENGTH 0.5 // segment length in mm
+#define MAX_NEGATIVE_ANGLE -45 // in negative radians how far can the arms go up before damaging machine (max pi/2) 
+
+// =================== Machine Hardware Definition =============
+
+#define DYNAMIXEL_TXD           GPIO_NUM_4
+#define DYNAMIXEL_RXD           GPIO_NUM_13
+#define DYNAMIXEL_RTS           GPIO_NUM_17
+
+#define X_DYNAMIXEL_ID          1 // protocol ID
+#define Y_DYNAMIXEL_ID          2 // protocol ID
+#define Z_DYNAMIXEL_ID          3 // protocol ID
+
+// limit servo to 180 degree motion
+#define DXL_COUNT_MIN           1024      
+#define DXL_COUNT_MAX           3072
+
+#define SERVO_TIMER_INTERVAL 50
+
+#define USER_DIGITAL_PIN_0      GPIO_NUM_25
+#define USER_DIGITAL_PIN_1      GPIO_NUM_26
+#define USER_DIGITAL_PIN_2      GPIO_NUM_27
+
+#define USER_ANALOG_PIN_0       GPIO_NUM_2
+#define USER_ANALOG_PIN_0_FREQ  50              // for use with RC servos duty range 5% to 10% 
+
+#define USER_ANALOG_PIN_1       GPIO_NUM_15
+#define USER_ANALOG_PIN_1_FREQ  50              // for use with RC servos duty range 5% to 10% 
+
+#define USER_ANALOG_PIN_2       GPIO_NUM_16
+#define USER_ANALOG_PIN_2_FREQ  50              // for use with RC servos duty range 5% to 10% 
+
+// ===================== Default Settings ==============================
+
+#define DEFAULT_STEP_PULSE_MICROSECONDS 3 
+#define DEFAULT_STEPPER_IDLE_LOCK_TIME 200 // 200ms
+
+#define DEFAULT_STATUS_REPORT_MASK 1
+
+#define DEFAULT_STEPPING_INVERT_MASK 0 // uint8_t
+#define DEFAULT_DIRECTION_INVERT_MASK 0 // uint8_t
+#define DEFAULT_INVERT_ST_ENABLE 0 // boolean
+
+#define DEFAULT_SOFT_LIMIT_ENABLE 0 // false
+#define DEFAULT_HARD_LIMIT_ENABLE 0  // false
+
+#define DEFAULT_HOMING_ENABLE 0
+#define DEFAULT_HOMING_DIR_MASK 0
+
+// the following must have the same values for each axis
+
+#define DEFAULT_X_STEPS_PER_MM 800 // value is actually arbitrary, but keep it smallish
+#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 200.0 // units/min
+#define DEFAULT_Y_MAX_RATE DEFAULT_X_MAX_RATE
+#define DEFAULT_Z_MAX_RATE DEFAULT_X_MAX_RATE
+
+#define DEFAULT_X_ACCELERATION      200.0 // 10*60*60 mm/min^2 = 10 mm/sec^2
+#define DEFAULT_Y_ACCELERATION      DEFAULT_X_ACCELERATION // 10*60*60 mm/min^2 = 10 mm/sec^2
+#define DEFAULT_Z_ACCELERATION      DEFAULT_X_ACCELERATION
+
+#define DEFAULT_X_MAX_TRAVEL 3.14159265 // 180° in radians 
+#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // 
+#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // 
+
+#define ARM_INTERNAL_ANGLE 2.866  // due to mounting angle 
+
+#define DEFAULT_X_HOMING_MPOS (DEFAULT_X_MAX_TRAVEL / 2.0)
+#define DEFAULT_Y_HOMING_MPOS DEFAULT_X_HOMING_MPOS
+#define DEFAULT_Z_HOMING_MPOS DEFAULT_X_HOMING_MPOS
+
+#define SPINDLE_TYPE    SpindleType::NONE

Grbl_Esp32/src/Machines/template.h

@@ -208,10 +208,10 @@
 // so non-Cartesian machines can be implemented.
 // #define USE_KINEMATICS
 
-// USE_FWD_KINEMATIC enables the forward_kinematics() function
+// USE_FWD_KINEMATICS enables the forward_kinematics() function
 // that converts motor positions in non-Cartesian coordinate
 // systems back to Cartesian form, for status reports.
-//#define USE_FWD_KINEMATIC
+//#define USE_FWD_KINEMATICS
 
 // USE_TOOL_CHANGE enables the user_tool_change() function
 // that implements custom tool change procedures.

Grbl_Esp32/src/Motors/Dynamixel2.cpp

@@ -41,6 +41,8 @@ namespace Motors {
 
         if (_tx_pin == UNDEFINED_PIN || _rx_pin == UNDEFINED_PIN || _rts_pin == UNDEFINED_PIN) {
             grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Dymanixel Error. Missing pin definitions");
+            is_active = false;
+
             return;
         }
 
@@ -58,7 +60,10 @@ namespace Motors {
 
         config_message();  // print the config
 
-        test();  // ping the motor
+        if (!test()) {  // ping the motor
+            is_active = false;
+            return;
+        }
 
         set_disable(true);                              // turn off torque so we can set EEPROM registers
         set_operating_mode(DXL_CONTROL_MODE_POSITION);  // set it in the right control mode
@@ -111,6 +116,7 @@ namespace Motors {
 
         } else {
             grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Axis Dynamixel Servo ID %d Ping failed", _axis_name, _id);
+            return false;
         }
 
         return true;
@@ -139,9 +145,10 @@ namespace Motors {
     void Dynamixel2::set_disable(bool disable) {
         uint8_t param_count = 1;
 
-        _disabled = disable;
+        if (_disabled == disable)
+            return;
 
-        //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Axis %s", _axis_name, disable ? "Disable" : "Enable");
+        _disabled = disable;
 
         if (_disabled)
             dxl_write(DXL_ADDR_TORQUE_EN, param_count, 0);
@@ -155,6 +162,9 @@ namespace Motors {
     }
 
     void Dynamixel2::update() {
+        if (!is_active)
+            return;
+
         if (_disabled) {
             dxl_read_position();
         } else {
@@ -223,8 +233,6 @@ namespace Motors {
 
     uint32_t Dynamixel2::dxl_read_position() {
         uint8_t data_len = 4;
-        //int32_t pos_min_steps, pos_max_steps;  // in steps
-        //int32_t dxl_position, dxl_count_min, dxl_count_max;
 
         dxl_read(DXL_PRESENT_POSITION, data_len);
 
@@ -239,11 +247,14 @@ namespace Motors {
             int32_t pos_min_steps = lround(_position_min * axis_settings[_axis_index]->steps_per_mm->get());
             int32_t pos_max_steps = lround(_position_max * axis_settings[_axis_index]->steps_per_mm->get());
 
-            sys_position[_axis_index] = map(dxl_position, DXL_COUNT_MIN, DXL_COUNT_MAX, pos_min_steps, pos_max_steps);
+            int32_t temp = map(dxl_position, DXL_COUNT_MIN, DXL_COUNT_MAX, pos_min_steps, pos_max_steps);
+
+            sys_position[_axis_index] = temp;
+
+            plan_sync_position();
 
             return dxl_position;
         } else {
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Data len arror: %d", data_len);
             return 0;
         }
     }
@@ -362,6 +373,7 @@ namespace Motors {
 
                     //determine the location of the axis
                     float target = system_convert_axis_steps_to_mpos(sys_position, axis);  // get the axis machine position in mm
+
                     float travel = axis_settings[axis]->max_travel->get();
                     float mpos   = axis_settings[axis]->home_mpos->get();
 
@@ -382,8 +394,6 @@ namespace Motors {
                     // map the mm range to the servo range
                     dxl_position = (uint32_t)mapConstrain(target, position_min, position_max, dxl_count_min, dxl_count_max);
 
-                    //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Range: %5.3f/%5.3f Target:%5.3f Pos %d", position_min, position_max, target, dxl_position);
-
                     tx_message[++msg_index] = current_id;                         // ID of the servo
                     tx_message[++msg_index] = dxl_position & 0xFF;                // data
                     tx_message[++msg_index] = (dxl_position & 0xFF00) >> 8;       // data

Grbl_Esp32/src/Motors/Dynamixel2.h

@@ -108,8 +108,7 @@ namespace Motors {
         static uint16_t dxl_update_crc(uint16_t crc_accum, char* data_blk_ptr, uint8_t data_blk_size);
         static void     dxl_bulk_goal_position();  // set all motorsd init_uart(uint8_t id, uint8_t axis_index, uint8_t dual_axis_index);
 
-        bool _disabled;
-
+        
         float _homing_position;
 
         float _dxl_count_min;

Grbl_Esp32/src/Motors/Motor.h

@@ -68,5 +68,6 @@ namespace Motors {
         float _position_max = 0;  // position in millimeters
 
         bool _can_home = true;
+        bool _disabled;
     };
 }

Grbl_Esp32/src/Motors/Motors.cpp

@@ -332,19 +332,16 @@ bool motors_have_type_id(motor_class_id_t id) {
 }
 
 void motors_set_disable(bool disable) {
-    static bool previous_state = false;
+    static bool previous_state = true;
 
+    //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Motors disable %d", disable);
+
+/*
     if (previous_state == disable) {
         return;
     }
-
-    previous_state = disable;
-
-    if (step_enable_invert->get()) {
-        disable = !disable;  // Apply pin invert.
-    }
-
-    digitalWrite(STEPPERS_DISABLE_PIN, disable);
+    previous_state = disable;    
+*/
 
     // now loop through all the motors to see if they can individually diable
     for (uint8_t gang_index = 0; gang_index < MAX_GANGED; gang_index++) {
@@ -352,6 +349,13 @@ void motors_set_disable(bool disable) {
             myMotor[axis][gang_index]->set_disable(disable);
         }
     }
+
+    // invert only inverts the global stepper disable pin.
+    if (step_enable_invert->get()) {
+        disable = !disable;  // Apply pin invert.
+    }
+    digitalWrite(STEPPERS_DISABLE_PIN, disable);
+    
 }
 
 void motors_read_settings() {

Grbl_Esp32/src/Motors/RcServo.h

@@ -42,7 +42,7 @@ namespace Motors {
         uint8_t  _pwm_pin;
         uint8_t  _channel_num;
         uint32_t _current_pwm_duty;
-        bool     _disabled;
+        
 
         float _homing_position;
 

Grbl_Esp32/src/Motors/TrinamicDriver.cpp

@@ -274,10 +274,15 @@ namespace Motors {
         if (has_errors)
             return;
 
-        digitalWrite(disable_pin, disable);
+        if (_disabled == disable)
+            return;
+
+        _disabled = disable;
+
+        digitalWrite(disable_pin, _disabled);
 
 #ifdef USE_TRINAMIC_ENABLE
-        if (disable) {
+        if (_disabled) {
             tmcstepper->toff(TRINAMIC_TOFF_DISABLE);
         } else {
             if (_mode == TRINAMIC_MODE_STEALTHCHOP) {

Grbl_Esp32/src/NutsBolts.cpp

@@ -196,6 +196,11 @@ float constrain_float(float in, float min, float max) {  // DrawBot_Badge
     return in;
 }
 
+long mapConstrain(long x, long in_min, long in_max, long out_min, long out_max) {
+    x = constrain(x, in_min, in_max);
+    return map(x, in_min, in_max, out_min, out_max);
+}
+
 float mapConstrain(float x, float in_min, float in_max, float out_min, float out_max) {
     x = constrain_float(x, in_min, in_max);
     return map_float(x, in_min, in_max, out_min, out_max);

Grbl_Esp32/src/Report.cpp

@@ -668,7 +668,7 @@ void report_realtime_status(uint8_t client) {
     if (bit_istrue(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE)) {
         strcat(status, "|MPos:");
     } else {
-#ifdef USE_FWD_KINEMATIC
+#ifdef USE_FWD_KINEMATICS        
         forward_kinematics(print_position);
 #endif
         strcat(status, "|WPos:");

Grbl_Esp32/src/SettingsDefinitions.cpp

@@ -328,7 +328,7 @@ void make_settings() {
     // TODO Settings - also need to clear, but not set, soft_limits
     arc_tolerance      = new FloatSetting(GRBL, WG, "12", "GCode/ArcTolerance", DEFAULT_ARC_TOLERANCE, 0, 1);
     junction_deviation = new FloatSetting(GRBL, WG, "11", "GCode/JunctionDeviation", DEFAULT_JUNCTION_DEVIATION, 0, 10);
-    status_mask        = new IntSetting(GRBL, WG, "10", "Report/Status", DEFAULT_STATUS_REPORT_MASK, 0, 2);
+    status_mask        = new IntSetting(GRBL, WG, "10", "Report/Status", DEFAULT_STATUS_REPORT_MASK, 0, 3);
 
     probe_invert           = new FlagSetting(GRBL, WG, "6", "Probe/Invert", DEFAULT_INVERT_PROBE_PIN);
     limit_invert           = new FlagSetting(GRBL, WG, "5", "Limits/Invert", DEFAULT_INVERT_LIMIT_PINS);

Grbl_Esp32/src/System.cpp

@@ -34,7 +34,7 @@ volatile uint8_t   sys_rt_exec_accessory_override;  // Global realtime executor
 volatile uint8_t sys_rt_exec_debug;
 #endif
 
-UserOutput::AnalogOutput* myAnalogOutputs[MaxUserDigitalPin];
+UserOutput::AnalogOutput*  myAnalogOutputs[MaxUserDigitalPin];
 UserOutput::DigitalOutput* myDigitalOutputs[MaxUserDigitalPin];
 
 xQueueHandle control_sw_queue;    // used by control switch debouncing
@@ -95,53 +95,17 @@ void system_ini() {  // Renamed from system_init() due to conflict with esp32 fi
     SPI.begin(GRBL_SPI_SCK, GRBL_SPI_MISO, GRBL_SPI_MOSI, GRBL_SPI_SS);
 #endif
 
-
-    // Setup USER_DIGITAL_PINs controlled by M62, M63, M64, & M65
-#ifdef USER_DIGITAL_PIN_0
+    // Setup M62,M63,M64,M65 pins
     myDigitalOutputs[0] = new UserOutput::DigitalOutput(0, USER_DIGITAL_PIN_0);
-#else
-    myDigitalOutputs[0] = new UserOutput::DigitalOutput();
-#endif
-#ifdef USER_DIGITAL_PIN_1
-    myDigitalOutputs[1] = new UserOutput::DigitalOutput(1, USER_DIGITAL_PIN_1);
-#else
-    myDigitalOutputs[1] = new UserOutput::DigitalOutput();
-#endif
-#ifdef USER_DIGITAL_PIN_2
-    myDigitalOutputs[2] = new UserOutput::DigitalOutput(2, USER_DIGITAL_PIN_2);
-#else
-    myDigitalOutputs[2] = new UserOutput::DigitalOutput();
-#endif
-#ifdef USER_DIGITAL_PIN_3
-    myDigitalOutputs[3] = new UserOutput::DigitalOutput(3, USER_DIGITAL_PIN_3);
-#else
-    myDigitalOutputs[3] = new UserOutput::DigitalOutput();
-#endif
+    myDigitalOutputs[0] = new UserOutput::DigitalOutput(1, USER_DIGITAL_PIN_1);
+    myDigitalOutputs[0] = new UserOutput::DigitalOutput(2, USER_DIGITAL_PIN_2);
+    myDigitalOutputs[0] = new UserOutput::DigitalOutput(3, USER_DIGITAL_PIN_3);
 
-// Setup M67 Pins
-#ifdef USER_ANALOG_PIN_0
+    // Setup M67 Pins
     myAnalogOutputs[0] = new UserOutput::AnalogOutput(0, USER_ANALOG_PIN_0, USER_ANALOG_PIN_0_FREQ);
-#else
-    myAnalogOutputs[0] = new UserOutput::AnalogOutput();
-#endif
-
-#ifdef USER_ANALOG_PIN_1
     myAnalogOutputs[1] = new UserOutput::AnalogOutput(1, USER_ANALOG_PIN_1, USER_ANALOG_PIN_1_FREQ);
-#else
-    myAnalogOutputs[1] = new UserOutput::AnalogOutput();
-#endif
-
-#ifdef USER_ANALOG_PIN_2
     myAnalogOutputs[2] = new UserOutput::AnalogOutput(2, USER_ANALOG_PIN_2, USER_ANALOG_PIN_2_FREQ);
-#else
-    myAnalogOutputs[2] = new UserOutput::AnalogOutput();
-#endif
-
-#ifdef USER_ANALOG_PIN_3
     myAnalogOutputs[3] = new UserOutput::AnalogOutput(3, USER_ANALOG_PIN_3, USER_ANALOG_PIN_3_FREQ);
-#else
-    myAnalogOutputs[3] = new UserOutput::AnalogOutput();
-#endif    
 }
 
 #ifdef ENABLE_CONTROL_SW_DEBOUNCE
@@ -169,7 +133,7 @@ void IRAM_ATTR isr_control_inputs() {
         xQueueSendFromISR(control_sw_queue, &evt, NULL);
     }
 #else
-    uint8_t pin        = system_control_get_state();
+    uint8_t pin = system_control_get_state();
     system_exec_control_pin(pin);
 #endif
 }
@@ -435,9 +399,6 @@ bool sys_pwm_control(uint8_t io_num_mask, float duty, bool synchronized) {
     return cmd_ok;
 }
 
-
-
-
 // Call this function to get an RMT channel number
 // returns -1 for error
 int8_t sys_get_next_RMT_chan_num() {

Grbl_Esp32/src/UserOutput.cpp

@@ -78,7 +78,8 @@ namespace UserOutput {
     }
 
     void AnalogOutput::config_message() {
-        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "User Analog Output:%d on Pin:%s", _number, pinName(_pin).c_str());
+        grbl_msg_sendf(
+            CLIENT_SERIAL, MsgLevel::Info, "User Analog Output:%d on Pin:%s Freq:%0.0fHz", _number, pinName(_pin).c_str(), _pwm_frequency);
     }
 
     // returns true if able to set value

Grbl_Esp32/src/user_io.md

@@ -57,12 +57,10 @@ Synchronized means all steps in the buffers must complete before the I/O is chan
 - Reset - All pins will be set to the off state
 - Alarm Mode - No change in pin state
 - Error - No change in pins state
-- End of File - TBD
+- End of File - No change in pins state
 
 ### Setup
 
-M62 - M65
-
 ```
 #define USER_DIGITAL_PIN_0   GPIO_NUM_xx
 #define USER_DIGITAL_PIN_1   GPIO_NUM_xx
@@ -79,14 +77,24 @@ M62 - M65
 
 ### Frequency
 
-Defining a frequency is optional. If you do not define one for a pin the default of 5000 will be used. If you are using the PWM to control an RC Servo, you should set it to something around 50. 
+Defining a frequency is optional. If you do not define one for a pin the default of 5000 will be used.
 
 ### Resolution
 
-The resolution is dependent on the frequency used. The PWM is based on a 80MHz timer. If you have a 10KHz frequency, you have 80,000,000 / 10,000 you have a maximum of an 8,000 count resolution. The resolution is based on bits so you need to round down to the nearest bit value which would be 14 bit or 4096. The highest bit value allowed is 13 bits.
+The resolution is dependent on the frequency used. The PWM is based on a 80MHz timer. If you have a 10KHz frequency, 80,000,000 / 10,000 gives you a maximum of an 8,000 count resolution. The resolution is based on bits so you need to round down to the nearest bit value which would be 12 bit or 4096. The highest bit value allowed is 13 bits.
 
 This is all done behind the scenes, you only have to provide the frequency and duty.
 
 ### Duty
 
-The duty is a percentage of the period. It you are looking for a specific pulse width, you need to determine the period, which is 1/freq. If your frequency is 100Hz  your period is 10ms. If you want a 1ms pulse, you would set the duty to 0.10%.
+The duty is a percentage of the period. It you are looking for a specific pulse width, you need to determine the period, which is 1/freq. If your frequency is 100Hz  your period is 10ms. If you want a 1ms pulse, you would set the duty to 0.10%. Duty is a floating point number. A value of XX.XX will get you the highest resolution. Values above 100% are set at 100%. Negative numbers are set to 0%. 
+
+### Use With RC Servos
+
+RC Servos set their position based on the pulse length of a PWM signal. The standard frequency for RC servos PWM is 50Hz, some digital servos can handle a higher rate, but it will not improve performance in this application and could overheat some servos.
+
+The standard pulse range for RC servos is 1ms to 2ms. Some servos have a wider range.
+
+With a 50Hz frequency the period is 20ms. Therefore a 1ms pulse is 5% duty and a 2ms pulse is 10% duty. Use the 2 values to to go from one end of travel to the other. You can use values in between to go to other positions. You can experiment with a little less or more if you servos have a bigger range.
+
+You can also set the duty to 0%. This generally turns a servo off and allows it to free wheel and not draw much power.