diff --git a/Grbl_Esp32/src/Machines/fystec_e4.h b/Grbl_Esp32/src/Machines/fystec_e4.h
new file mode 100644
index 00000000..63b47bab
--- /dev/null
+++ b/Grbl_Esp32/src/Machines/fystec_e4.h
@@ -0,0 +1,81 @@
+#pragma once
+// clang-format off
+
+/*
+ fystec_e4.h
+ https://github.com/FYSETC/FYSETC-E4
+
+ 2020-12-03 B. Dring
+
+ This is a machine definition file to use the FYSTEC E4 3D Printer controller
+ This is a 4 motor controller. This is setup for XYZA, but XYYZ, could also be used.
+ There are 5 inputs
+ The controller has outputs for a Fan, Hotbed and Extruder. There are mapped to
+ spindle, mist and flood coolant to drive an external relay.
+
+ 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 .
+*/
+
+#define MACHINE_NAME "FYSTEC E4 3D Printer Controller"
+
+#define N_AXIS 4
+
+#define TRINAMIC_RUN_MODE TrinamicMode :: StealthChop
+#define TRINAMIC_HOMING_MODE TrinamicMode :: StealthChop
+
+#define TMC_UART UART_NUM_1
+#define TMC_UART_RX GPIO_NUM_21
+#define TMC_UART_TX GPIO_NUM_22
+
+#define X_TRINAMIC_DRIVER 2209
+#define X_STEP_PIN GPIO_NUM_27
+#define X_DIRECTION_PIN GPIO_NUM_26
+#define X_RSENSE TMC2209_RSENSE_DEFAULT
+#define X_DRIVER_ADDRESS 1
+#define DEFAULT_X_MICROSTEPS 16
+
+#define Y_TRINAMIC_DRIVER 2209
+#define Y_STEP_PIN GPIO_NUM_33
+#define Y_DIRECTION_PIN GPIO_NUM_32
+#define Y_RSENSE TMC2209_RSENSE_DEFAULT
+#define Y_DRIVER_ADDRESS 3
+#define DEFAULT_Y_MICROSTEPS 16
+
+#define Z_TRINAMIC_DRIVER 2209
+#define Z_STEP_PIN GPIO_NUM_14
+#define Z_DIRECTION_PIN GPIO_NUM_12
+#define Z_RSENSE TMC2209_RSENSE_DEFAULT
+#define Z_DRIVER_ADDRESS 0
+#define DEFAULT_Z_MICROSTEPS 16
+
+#define A_TRINAMIC_DRIVER 2209
+#define A_STEP_PIN GPIO_NUM_16
+#define A_DIRECTION_PIN GPIO_NUM_17
+#define A_RSENSE TMC2209_RSENSE_DEFAULT
+#define A_DRIVER_ADDRESS 2
+#define DEFAULT_A_MICROSTEPS 16
+
+#define X_LIMIT_PIN GPIO_NUM_34
+#define Y_LIMIT_PIN GPIO_NUM_35
+#define Z_LIMIT_PIN GPIO_NUM_15
+#define A_LIMIT_PIN GPIO_NUM_36 // Labeled TB
+#define PROBE_PIN GPIO_NUM_39 // Labeled TE
+
+// OK to comment out to use pin for other features
+#define STEPPERS_DISABLE_PIN GPIO_NUM_25
+
+#define SPINDLE_TYPE SpindleType::RELAY
+#define SPINDLE_OUTPUT_PIN GPIO_NUM_13 // labeled Fan
+#define COOLANT_MIST_PIN GPIO_NUM_2 // Labeled Hotbed
+#define COOLANT_FLOOD_PIN GPIO_NUM_4 // Labeled Heater
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp
index 1a08c0c8..1c06a7d7 100644
--- a/Grbl_Esp32/src/Motors/Motors.cpp
+++ b/Grbl_Esp32/src/Motors/Motors.cpp
@@ -42,6 +42,7 @@
#include "RcServo.h"
#include "Dynamixel2.h"
#include "TrinamicDriver.h"
+#include "TrinamicUartDriver.h"
Motors::Motor* myMotor[MAX_AXES][MAX_GANGED]; // number of axes (normal and ganged)
void init_motors() {
@@ -51,8 +52,19 @@ void init_motors() {
if (n_axis >= 1) {
#ifdef X_TRINAMIC_DRIVER
- myMotor[X_AXIS][0] =
- new Motors::TrinamicDriver(X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[X_AXIS][0] =
+ new Motors::TrinamicDriver(X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE);
+ }
+# elif (X_TRINAMIC_DRIVER == 2208 || X_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[X_AXIS][0] = new Motors::TrinamicUartDriver(
+ X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_TRINAMIC_DRIVER, X_RSENSE, X_DRIVER_ADDRESS);
+ }
+# else
+# error X Axis undefined motor p/n
+# endif
#elif defined(X_SERVO_PIN)
myMotor[X_AXIS][0] = new Motors::RcServo(X_AXIS, X_SERVO_PIN);
#elif defined(X_UNIPOLAR)
@@ -66,8 +78,19 @@ void init_motors() {
#endif
#ifdef X2_TRINAMIC_DRIVER
- myMotor[X_AXIS][1] =
- new Motors::TrinamicDriver(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[X_AXIS][1] =
+ new Motors::TrinamicDriver(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE);
+ }
+# elif (X2_TRINAMIC_DRIVER == 2208 || X2_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[X_AXIS][1] = new Motors::TrinamicUartDriver(
+ X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE, X2_DRIVER_ADDRESS);
+ }
+# else
+# error X2 Axis undefined motor p/n
+# endif
#elif defined(X2_UNIPOLAR)
myMotor[X_AXIS][1] = new Motors::UnipolarMotor(X2_AXIS, X2_PIN_PHASE_0, X2_PIN_PHASE_1, X2_PIN_PHASE_2, X2_PIN_PHASE_3);
#elif defined(X2_STEP_PIN)
@@ -83,8 +106,19 @@ void init_motors() {
if (n_axis >= 2) {
// this WILL be done better with settings
#ifdef Y_TRINAMIC_DRIVER
- myMotor[Y_AXIS][0] =
- new Motors::TrinamicDriver(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[Y_AXIS][0] =
+ new Motors::TrinamicDriver(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE);
+ }
+# elif (Y_TRINAMIC_DRIVER == 2208 || Y_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[Y_AXIS][0] = new Motors::TrinamicUartDriver(
+ Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE, Y_DRIVER_ADDRESS);
+ }
+# else
+# error Y Axis undefined motor p/n
+# endif
#elif defined(Y_SERVO_PIN)
myMotor[Y_AXIS][0] = new Motors::RcServo(Y_AXIS, Y_SERVO_PIN);
#elif defined(Y_UNIPOLAR)
@@ -98,8 +132,19 @@ void init_motors() {
#endif
#ifdef Y2_TRINAMIC_DRIVER
- myMotor[Y_AXIS][1] =
- new Motors::TrinamicDriver(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[Y_AXIS][1] =
+ new Motors::TrinamicDriver(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE);
+ }
+# elif (Y2_TRINAMIC_DRIVER == 2208 || Y2_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[Y_AXIS][1] = new Motors::TrinamicUartDriver(
+ Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE, Y2_DRIVER_ADDRESS);
+ }
+# else
+# error Y2 Axis undefined motor p/n
+# endif
#elif defined(Y2_UNIPOLAR)
myMotor[Y_AXIS][1] = new Motors::UnipolarMotor(Y2_AXIS, Y2_PIN_PHASE_0, Y2_PIN_PHASE_1, Y2_PIN_PHASE_2, Y2_PIN_PHASE_3);
#elif defined(Y2_STEP_PIN)
@@ -115,8 +160,19 @@ void init_motors() {
if (n_axis >= 3) {
// this WILL be done better with settings
#ifdef Z_TRINAMIC_DRIVER
- myMotor[Z_AXIS][0] =
- new Motors::TrinamicDriver(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[Z_AXIS][0] =
+ new Motors::TrinamicDriver(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE);
+ }
+# elif (Z_TRINAMIC_DRIVER == 2208 || Z_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[Z_AXIS][0] = new Motors::TrinamicUartDriver(
+ Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE, Z_DRIVER_ADDRESS);
+ }
+# else
+# error Z Axis undefined motor p/n
+# endif
#elif defined(Z_SERVO_PIN)
myMotor[Z_AXIS][0] = new Motors::RcServo(Z_AXIS, Z_SERVO_PIN);
#elif defined(Z_UNIPOLAR)
@@ -130,8 +186,19 @@ void init_motors() {
#endif
#ifdef Z2_TRINAMIC_DRIVER
- myMotor[Z_AXIS][1] =
- new Motors::TrinamicDriver(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[Z_AXIS][1] =
+ new Motors::TrinamicDriver(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE);
+ }
+# elif (Z2_TRINAMIC_DRIVER == 2208 || Z2_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[Z_AXIS][1] = new Motors::TrinamicUartDriver(
+ Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE, Z2_DRIVER_ADDRESS);
+ }
+# else
+# error Z2 Axis undefined motor p/n
+# endif
#elif defined(Z2_UNIPOLAR)
myMotor[Z_AXIS][1] = new Motors::UnipolarMotor(Z2_AXIS, Z2_PIN_PHASE_0, Z2_PIN_PHASE_1, Z2_PIN_PHASE_2, Z2_PIN_PHASE_3);
#elif defined(Z2_STEP_PIN)
@@ -147,8 +214,19 @@ void init_motors() {
if (n_axis >= 4) {
// this WILL be done better with settings
#ifdef A_TRINAMIC_DRIVER
- myMotor[A_AXIS][0] =
- new Motors::TrinamicDriver(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[A_AXIS][1] =
+ new Motors::TrinamicDriver(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE);
+ }
+# elif (A_TRINAMIC_DRIVER == 2208 || A_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[A_AXIS][0] = new Motors::TrinamicUartDriver(
+ A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_TRINAMIC_DRIVER, A_RSENSE, A_DRIVER_ADDRESS);
+ }
+# else
+# error A Axis undefined motor p/n
+# endif
#elif defined(A_SERVO_PIN)
myMotor[A_AXIS][0] = new Motors::RcServo(A_AXIS, A_SERVO_PIN);
#elif defined(A_UNIPOLAR)
@@ -162,8 +240,19 @@ void init_motors() {
#endif
#ifdef A2_TRINAMIC_DRIVER
- myMotor[A_AXIS][1] =
- new Motors::TrinamicDriver(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[A_AXIS][1] =
+ new Motors::TrinamicDriver(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE);
+ }
+# elif (A2_TRINAMIC_DRIVER == 2208 || A2_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[A_AXIS][1] = new Motors::TrinamicUartDriver(
+ A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE, A2_DRIVER_ADDRESS);
+ }
+# else
+# error A2 Axis undefined motor p/n
+# endif
#elif defined(A2_UNIPOLAR)
myMotor[A_AXIS][1] = new Motors::UnipolarMotor(A2_AXIS, A2_PIN_PHASE_0, A2_PIN_PHASE_1, A2_PIN_PHASE_2, A2_PIN_PHASE_3);
#elif defined(A2_STEP_PIN)
@@ -179,8 +268,19 @@ void init_motors() {
if (n_axis >= 5) {
// this WILL be done better with settings
#ifdef B_TRINAMIC_DRIVER
- myMotor[B_AXIS][0] =
- new Motors::TrinamicDriver(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[B_AXIS][1] =
+ new Motors::TrinamicDriver(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE);
+ }
+# elif (B_TRINAMIC_DRIVER == 2208 || B_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[B_AXIS][0] = new Motors::TrinamicUartDriver(
+ B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_TRINAMIC_DRIVER, B_RSENSE, B_DRIVER_ADDRESS);
+ }
+# else
+# error B Axis undefined motor p/n
+# endif
#elif defined(B_SERVO_PIN)
myMotor[B_AXIS][0] = new Motors::RcServo(B_AXIS, B_SERVO_PIN);
#elif defined(B_UNIPOLAR)
@@ -194,8 +294,19 @@ void init_motors() {
#endif
#ifdef B2_TRINAMIC_DRIVER
- myMotor[B_AXIS][1] =
- new Motors::TrinamicDriver(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[B_AXIS][1] =
+ new Motors::TrinamicDriver(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE);
+ }
+# elif (B2_TRINAMIC_DRIVER == 2208 || B2_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[B_AXIS][1] = new Motors::TrinamicUartDriver(
+ B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE, B2_DRIVER_ADDRESS);
+ }
+# else
+# error B2 Axis undefined motor p/n
+# endif
#elif defined(B2_UNIPOLAR)
myMotor[B_AXIS][1] = new Motors::UnipolarMotor(B2_AXIS, B2_PIN_PHASE_0, B2_PIN_PHASE_1, B2_PIN_PHASE_2, B2_PIN_PHASE_3);
#elif defined(B2_STEP_PIN)
@@ -211,8 +322,19 @@ void init_motors() {
if (n_axis >= 6) {
// this WILL be done better with settings
#ifdef C_TRINAMIC_DRIVER
- myMotor[C_AXIS][0] =
- new Motors::TrinamicDriver(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[C_AXIS][1] =
+ new Motors::TrinamicDriver(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE);
+ }
+# elif (C_TRINAMIC_DRIVER == 2208 || C_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[C_AXIS][0] = new Motors::TrinamicUartDriver(
+ C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_TRINAMIC_DRIVER, C_RSENSE, C_DRIVER_ADDRESS);
+ }
+# else
+# error C Axis undefined motor p/n
+# endif
#elif defined(C_SERVO_PIN)
myMotor[C_AXIS][0] = new Motors::RcServo(C_AXIS, C_SERVO_PIN);
#elif defined(C_UNIPOLAR)
@@ -226,8 +348,19 @@ void init_motors() {
#endif
#ifdef C2_TRINAMIC_DRIVER
- myMotor[C_AXIS][1] =
- new Motors::TrinamicDriver(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE);
+# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160)
+ {
+ myMotor[C_AXIS][1] =
+ new Motors::TrinamicDriver(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE);
+ }
+# elif (C2_TRINAMIC_DRIVER == 2208 || C2_TRINAMIC_DRIVER == 2209)
+ {
+ myMotor[C_AXIS][1] = new Motors::TrinamicUartDriver(
+ C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE, C2_DRIVER_ADDRESS);
+ }
+# else
+# error C2 Axis undefined motor p/n
+# endif
#elif defined(C2_UNIPOLAR)
myMotor[C_AXIS][1] = new Motors::UnipolarMotor(C2_AXIS, C2_PIN_PHASE_0, C2_PIN_PHASE_1, C2_PIN_PHASE_2, C2_PIN_PHASE_3);
#elif defined(C2_STEP_PIN)
@@ -360,4 +493,4 @@ void motors_unstep() {
myMotor[axis][0]->unstep();
myMotor[axis][1]->unstep();
}
-}
+}
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Motors/TrinamicUartDriver.h b/Grbl_Esp32/src/Motors/TrinamicUartDriver.h
new file mode 100644
index 00000000..3fb6e7cd
--- /dev/null
+++ b/Grbl_Esp32/src/Motors/TrinamicUartDriver.h
@@ -0,0 +1,131 @@
+#pragma once
+
+/*
+ TrinamicUartDriver.h
+
+ Part of Grbl_ESP32
+ 2020 - The Ant Team
+ 2020 - Bart Dring
+
+ 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 .
+*/
+
+#include "Motor.h"
+#include "StandardStepper.h"
+
+#include // https://github.com/teemuatlut/TMCStepper
+
+const float TMC2208_RSENSE_DEFAULT = 0.11f;
+const float TMC2209_RSENSE_DEFAULT = 0.11f;
+
+const double TRINAMIC_UART_FCLK = 12700000.0; // Internal clock Approx (Hz) used to calculate TSTEP from homing rate
+
+// ==== defaults OK to define them in your machine definition ====
+
+#ifndef TRINAMIC_UART_RUN_MODE
+# define TRINAMIC_UART_RUN_MODE TrinamicUartMode ::StealthChop
+#endif
+
+#ifndef TRINAMIC_UART_HOMING_MODE
+# define TRINAMIC_UART_HOMING_MODE TRINAMIC_UART_RUN_MODE
+#endif
+
+#ifndef TRINAMIC_UART_TOFF_DISABLE
+# define TRINAMIC_UART_TOFF_DISABLE 0
+#endif
+
+#ifndef TRINAMIC_UART_TOFF_STEALTHCHOP
+# define TRINAMIC_UART_TOFF_STEALTHCHOP 5
+#endif
+
+#ifndef TRINAMIC_UART_TOFF_COOLSTEP
+# define TRINAMIC_UART_TOFF_COOLSTEP 3
+#endif
+
+#ifndef TMC_UART
+# define TMC_UART UART_NUM_2
+#endif
+
+#ifndef TMC_UART_RX
+# define TMC_UART_RX UNDEFINED_PIN
+#endif
+
+#ifndef TMC_UART_TX
+# define TMC_UART_TX UNDEFINED_PIN
+#endif
+
+extern HardwareSerial tmc_serial;
+
+namespace Motors {
+
+ enum class TrinamicUartMode : uint8_t {
+ None = 0, // not for machine defs!
+ StealthChop = 1,
+ CoolStep = 2,
+ StallGuard = 3,
+ };
+
+ class TrinamicUartDriver : public StandardStepper {
+ public:
+ TrinamicUartDriver(uint8_t axis_index,
+ uint8_t step_pin,
+ uint8_t dir_pin,
+ uint8_t disable_pin,
+ uint16_t driver_part_number,
+ float r_senseS,
+ uint8_t address);
+
+ void config_message();
+ void hw_serial_init();
+ void init();
+ void set_mode();
+ void read_settings();
+ void debug_message();
+ bool set_homing_mode(bool is_homing) override;
+ void set_disable(bool disable) override;
+
+ uint8_t addr;
+
+ private:
+ uint32_t calc_tstep(float speed, float percent); //TODO: see if this is useful/used.
+
+ TMC2208Stepper* tmcstepper; // all other driver types are subclasses of this one
+ TrinamicUartMode _homing_mode;
+ uint16_t _driver_part_number; // example: use 2209 for TMC2209
+ float _r_sense;
+ bool _has_errors;
+ bool _disabled;
+
+ TrinamicUartMode _mode = TrinamicUartMode::None;
+ bool test();
+ void set_mode(bool isHoming);
+ void trinamic_test_response();
+ void trinamic_stepper_enable(bool enable);
+
+ bool report_open_load(TMC2208_n ::DRV_STATUS_t status);
+ bool report_short_to_ground(TMC2208_n ::DRV_STATUS_t status);
+ bool report_over_temp(TMC2208_n ::DRV_STATUS_t status);
+ bool report_short_to_ps(TMC2208_n ::DRV_STATUS_t status);
+
+ uint8_t get_next_index();
+
+ // Linked list of Trinamic driver instances, used by the
+ // StallGuard reporting task. TODO: verify if this is really used/useful.
+ static TrinamicUartDriver* List;
+ TrinamicUartDriver* link;
+ static void readSgTask(void*);
+
+ protected:
+ // void config_message() override;
+ };
+
+}
\ No newline at end of file
diff --git a/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp b/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp
new file mode 100644
index 00000000..e930a848
--- /dev/null
+++ b/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp
@@ -0,0 +1,370 @@
+/*
+ TrinamicUartDriverClass.cpp
+
+ This is used for Trinamic UART controlled stepper motor drivers.
+
+ Part of Grbl_ESP32
+ 2020 - The Ant Team
+ 2020 - Bart Dring
+
+ 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 .
+
+*/
+#include "TrinamicUartDriver.h"
+
+#include
+
+HardwareSerial tmc_serial(TMC_UART);
+
+namespace Motors {
+
+ /* HW Serial Constructor. */
+ TrinamicUartDriver::TrinamicUartDriver(
+ uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin, uint16_t driver_part_number, float r_sense, uint8_t addr) :
+ StandardStepper(axis_index, step_pin, dir_pin, disable_pin) {
+ _driver_part_number = driver_part_number;
+ _has_errors = false;
+ _r_sense = r_sense;
+ this->addr = addr;
+
+ uart_set_pin(TMC_UART, TMC_UART_TX, TMC_UART_RX, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+ tmc_serial.begin(115200, SERIAL_8N1, TMC_UART_RX, TMC_UART_TX);
+ tmc_serial.setRxBufferSize(128);
+ hw_serial_init();
+ }
+
+ void TrinamicUartDriver::hw_serial_init() {
+ if (_driver_part_number == 2208)
+ // TMC 2208 does not use address, this field is 0
+ tmcstepper = new TMC2208Stepper(&tmc_serial, _r_sense);
+ else if (_driver_part_number == 2209)
+ tmcstepper = new TMC2209Stepper(&tmc_serial, _r_sense, addr);
+ else {
+ grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Unsupported Trinamic motor p/n:%d", _driver_part_number);
+ return;
+ }
+ }
+
+ void TrinamicUartDriver ::init() {
+ if (_has_errors) {
+ return;
+ }
+
+ init_step_dir_pins(); // from StandardStepper
+ config_message();
+
+ tmcstepper->begin();
+
+ _has_errors = !test(); // Try communicating with motor. Prints an error if there is a problem.
+
+ /* If communication with the driver is working, read the
+ main settings, apply new driver settings and then read
+ them back. */
+ if (!_has_errors) { //TODO: verify if this is the right way to set the Irun/IHold and microsteps.
+ read_settings();
+ set_mode(false);
+ }
+ }
+
+ /*
+ This is the startup message showing the basic definition.
+ */
+ void TrinamicUartDriver::config_message() { //TODO: The RX/TX pin could be added to the msg.
+ grbl_msg_sendf(CLIENT_SERIAL,
+ MsgLevel::Info,
+ "%s motor Trinamic TMC%d Step:%s Dir:%s Disable:%s UART%d Rx:%s Tx:%s Addr:%d R:%0.3f %s",
+ reportAxisNameMsg(_axis_index, _dual_axis_index),
+ _driver_part_number,
+ pinName(_step_pin).c_str(),
+ pinName(_dir_pin).c_str(),
+ pinName(_disable_pin).c_str(),
+ TMC_UART,
+ pinName(TMC_UART_RX),
+ pinName(TMC_UART_TX),
+ this->addr,
+ _r_sense,
+ reportAxisLimitsMsg(_axis_index));
+ }
+
+ bool TrinamicUartDriver::test() {
+ if (_has_errors) {
+ return false;
+ }
+ switch (tmcstepper->test_connection()) {
+ case 1:
+ grbl_msg_sendf(CLIENT_SERIAL,
+ MsgLevel::Info,
+ "%s driver test failed. Check connection",
+ reportAxisNameMsg(_axis_index, _dual_axis_index));
+ return false;
+ case 2:
+ grbl_msg_sendf(CLIENT_SERIAL,
+ MsgLevel::Info,
+ "%s driver test failed. Check motor power",
+ reportAxisNameMsg(_axis_index, _dual_axis_index));
+ return false;
+ default:
+ // driver responded, so check for other errors from the DRV_STATUS register
+
+ TMC2208_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits.
+ status.sr = tmcstepper->DRV_STATUS();
+
+ bool err = false;
+
+ // look for errors
+ if (report_short_to_ground(status)) {
+ err = true;
+ }
+
+ if (report_over_temp(status)) {
+ err = true;
+ }
+
+ if (report_short_to_ps(status)) {
+ err = true;
+ }
+
+ if (err) {
+ return false;
+ }
+
+ grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s driver test passed", reportAxisNameMsg(_axis_index, _dual_axis_index));
+ return true;
+ }
+ }
+
+ /*
+ Read setting and send them to the driver. Called at init() and whenever related settings change
+ both are stored as float Amps, but TMCStepper library expects...
+ uint16_t run (mA)
+ float hold (as a percentage of run)
+ */
+ void TrinamicUartDriver::read_settings() {
+ if (_has_errors) {
+ return;
+ }
+
+ uint16_t run_i_ma = (uint16_t)(axis_settings[_axis_index]->run_current->get() * 1000.0);
+ float hold_i_percent;
+
+ if (axis_settings[_axis_index]->run_current->get() == 0)
+ hold_i_percent = 0;
+ else {
+ hold_i_percent = axis_settings[_axis_index]->hold_current->get() / axis_settings[_axis_index]->run_current->get();
+ if (hold_i_percent > 1.0)
+ hold_i_percent = 1.0;
+ }
+ tmcstepper->microsteps(axis_settings[_axis_index]->microsteps->get());
+ tmcstepper->rms_current(run_i_ma, hold_i_percent);
+
+ // grbl_msg_sendf(CLIENT_SERIAL,
+ // MsgLevel::Info,
+ // "Setting current of driver %s, target: %u, read irun: %d, hold percent: %f, usteps: %d",
+ // reportAxisNameMsg(_axis_index, _dual_axis_index), run_i_ma, tmcstepper->rms_current(), hold_i_percent, axis_settings[_axis_index]->microsteps->get());
+ }
+
+ bool TrinamicUartDriver::set_homing_mode(bool isHoming) {
+ set_mode(isHoming);
+ return true;
+ }
+
+ /*
+ There are ton of settings. I'll start by grouping then into modes for now.
+ Many people will want quiet and stallgaurd homing. Stallguard only run in
+ Coolstep mode, so it will need to switch to Coolstep when homing
+ */
+ void TrinamicUartDriver::set_mode(bool isHoming) {
+ if (_has_errors) {
+ return;
+ }
+
+ TrinamicUartMode newMode = isHoming ? TRINAMIC_UART_HOMING_MODE : TRINAMIC_UART_RUN_MODE;
+
+ if (newMode == _mode) {
+ return;
+ }
+ _mode = newMode;
+
+ switch (_mode) {
+ case TrinamicUartMode ::StealthChop:
+ //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "StealthChop");
+ // tmcstepper->en_pwm_mode(true); //TODO: check if this is present in TMC2208/09
+ tmcstepper->en_spreadCycle(false);
+ tmcstepper->pwm_autoscale(true);
+ // if (_driver_part_number == 2209) {
+ // tmcstepper->diag1_stall(false); //TODO: check the equivalent in TMC2209
+ // }
+ break;
+ case TrinamicUartMode ::CoolStep:
+ //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Coolstep");
+ // tmcstepper->en_pwm_mode(false); //TODO: check if this is present in TMC2208/09
+ tmcstepper->en_spreadCycle(true);
+ tmcstepper->pwm_autoscale(false);
+ if (_driver_part_number == 2209) {
+ // tmcstepper->TCOOLTHRS(NORMAL_TCOOLTHRS); // when to turn on coolstep //TODO: add this solving compilation issue.
+ // tmcstepper->THIGH(NORMAL_THIGH); //TODO: this does not exist in TMC2208/09. verify and eventually remove.
+ }
+ break;
+ case TrinamicUartMode ::StallGuard: //TODO: check all configurations for stallguard
+ //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard");
+ // tmcstepper->en_pwm_mode(false); //TODO: check if this is present in TMC2208/09
+ tmcstepper->en_spreadCycle(false);
+ tmcstepper->pwm_autoscale(false);
+ // tmcstepper->TCOOLTHRS(calc_tstep(homing_feed_rate->get(), 150.0));
+ // tmcstepper->THIGH(calc_tstep(homing_feed_rate->get(), 60.0));
+ // tmcstepper->sfilt(1);
+ // tmcstepper->diag1_stall(true); // stallguard i/o is on diag1
+ // tmcstepper->sgt(axis_settings[_axis_index]->stallguard->get());
+ break;
+ default:
+ grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Unknown Trinamic mode:d", _mode);
+ }
+ }
+
+ /*
+ This is the stallguard tuning info. It is call debug, so it could be generic across all classes.
+ */
+ void TrinamicUartDriver::debug_message() {
+ if (_has_errors) {
+ return;
+ }
+ uint32_t tstep = tmcstepper->TSTEP();
+
+ if (tstep == 0xFFFFF || tstep < 1) { // if axis is not moving return
+ return;
+ }
+ float feedrate = st_get_realtime_rate(); //* settings.microsteps[axis_index] / 60.0 ; // convert mm/min to Hz
+
+ grbl_msg_sendf(CLIENT_SERIAL,
+ MsgLevel::Info,
+ "%s Stallguard %d SG_Val: %04d Rate: %05.0f mm/min SG_Setting:%d",
+ reportAxisNameMsg(_axis_index, _dual_axis_index),
+ 0, // tmcstepper->stallguard(), // TODO: add this again solving the compilation issues
+ 0, // tmcstepper->sg_result(),
+ feedrate,
+ axis_settings[_axis_index]->stallguard->get());
+
+ TMC2208_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits.
+ status.sr = tmcstepper->DRV_STATUS();
+
+ // these only report if there is a fault condition
+ report_open_load(status);
+ report_short_to_ground(status);
+ report_over_temp(status);
+ report_short_to_ps(status);
+
+ // grbl_msg_sendf(CLIENT_SERIAL,
+ // MsgLevel::Info,
+ // "%s Status Register %08x GSTAT %02x",
+ // reportAxisNameMsg(_axis_index, _dual_axis_index),
+ // status.sr,
+ // tmcstepper->GSTAT());
+ }
+
+ // calculate a tstep from a rate
+ // tstep = TRINAMIC_UART_FCLK / (time between 1/256 steps)
+ // This is used to set the stallguard window from the homing speed.
+ // The percent is the offset on the window
+ uint32_t TrinamicUartDriver::calc_tstep(float speed, float percent) {
+ float tstep =
+ speed / 60.0 * axis_settings[_axis_index]->steps_per_mm->get() * (float)(256 / axis_settings[_axis_index]->microsteps->get());
+ tstep = TRINAMIC_UART_FCLK / tstep * percent / 100.0;
+
+ return static_cast(tstep);
+ }
+
+ // this can use the enable feature over SPI. The dedicated pin must be in the enable mode,
+ // but that can be hardwired that way.
+ void TrinamicUartDriver::set_disable(bool disable) {
+ if (_has_errors) {
+ return;
+ }
+
+ if (_disabled == disable) {
+ return;
+ }
+
+ _disabled = disable;
+
+ digitalWrite(_disable_pin, _disabled);
+
+#ifdef USE_TRINAMIC_ENABLE
+ if (_disabled) {
+ tmcstepper->toff(TRINAMIC_UART_TOFF_DISABLE);
+ } else {
+ if (_mode == TrinamicUartMode::StealthChop) {
+ tmcstepper->toff(TRINAMIC_UART_TOFF_STEALTHCHOP);
+ } else {
+ tmcstepper->toff(TRINAMIC_UART_TOFF_COOLSTEP);
+ }
+ }
+#endif
+ // the pin based enable could be added here.
+ // This would be for individual motors, not the single pin for all motors.
+ }
+
+ // =========== Reporting functions ========================
+
+ bool TrinamicUartDriver::report_open_load(TMC2208_n ::DRV_STATUS_t status) {
+ if (status.ola || status.olb) {
+ grbl_msg_sendf(CLIENT_SERIAL,
+ MsgLevel::Info,
+ "%s Driver open load A:%s B:%s",
+ reportAxisNameMsg(_axis_index, _dual_axis_index),
+ status.ola ? "Y" : "N",
+ status.olb ? "Y" : "N");
+ return true;
+ }
+ return false; // no error
+ }
+
+ bool TrinamicUartDriver::report_short_to_ground(TMC2208_n ::DRV_STATUS_t status) {
+ if (status.s2ga || status.s2gb) {
+ grbl_msg_sendf(CLIENT_SERIAL,
+ MsgLevel::Info,
+ "%s Driver shorted coil A:%s B:%s",
+ reportAxisNameMsg(_axis_index, _dual_axis_index),
+ status.s2ga ? "Y" : "N",
+ status.s2gb ? "Y" : "N");
+ return true;
+ }
+ return false; // no error
+ }
+
+ bool TrinamicUartDriver::report_over_temp(TMC2208_n ::DRV_STATUS_t status) {
+ if (status.ot || status.otpw) {
+ grbl_msg_sendf(CLIENT_SERIAL,
+ MsgLevel::Info,
+ "%s Driver temp Warning:%s Fault:%s",
+ reportAxisNameMsg(_axis_index, _dual_axis_index),
+ status.otpw ? "Y" : "N",
+ status.ot ? "Y" : "N");
+ return true;
+ }
+ return false; // no error
+ }
+
+ bool TrinamicUartDriver::report_short_to_ps(TMC2208_n ::DRV_STATUS_t status) {
+ // check for short to power supply
+ if ((status.sr & bit(12)) || (status.sr & bit(13))) {
+ grbl_msg_sendf(CLIENT_SERIAL,
+ MsgLevel::Info,
+ "%s Driver short vsa:%s vsb:%s",
+ reportAxisNameMsg(_axis_index, _dual_axis_index),
+ (status.sr & bit(12)) ? "Y" : "N",
+ (status.sr & bit(13)) ? "Y" : "N");
+ return true;
+ }
+ return false; // no error
+ }
+
+}
\ No newline at end of file