diff --git a/Grbl_Esp32/Machines/motor_class_test.h b/Grbl_Esp32/Machines/motor_class_test.h index 9d2da91d..c6b94df0 100644 --- a/Grbl_Esp32/Machines/motor_class_test.h +++ b/Grbl_Esp32/Machines/motor_class_test.h @@ -249,5 +249,9 @@ #define Z_DIRECTION_PIN GPIO_NUM_32 #define Z_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin +#define A_SERVO +#define A_SERVO_PIN GPIO_NUM_33 + + #define SPINDLE_TYPE SPINDLE_TYPE_NONE diff --git a/Grbl_Esp32/Motors/MotorClass.cpp b/Grbl_Esp32/Motors/MotorClass.cpp index 32942895..ad19e831 100644 --- a/Grbl_Esp32/Motors/MotorClass.cpp +++ b/Grbl_Esp32/Motors/MotorClass.cpp @@ -41,6 +41,7 @@ #include "TrinamicDriverClass.cpp" #include "StandardStepperClass.cpp" #include "UnipolarMotorClass.cpp" +#include "RcServoClass.cpp" Motor* myMotor[6][2]; // number of axes (normal and ganged) static TaskHandle_t readSgTaskHandle = 0; // for realtime stallguard data diaplay @@ -60,130 +61,164 @@ void init_motors() { // this WILL be done better with settings #ifdef X_TRINAMIC_DRIVER myMotor[X_AXIS][0] = new TrinamicDriver(X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_TRINAMIC_DRIVER, X_RSENSE, X_CS_PIN, get_next_trinamic_driver_index()); -#else - #ifdef X_UNIPOLAR +#elif defined(X_SERVO) + myMotor[X_AXIS][0] = new RcServo(X_AXIS, X_SERVO_PIN); +#elif defined(X_UNIPOLAR) myMotor[X_AXIS][0] = new UnipolarMotor(X_AXIS, X_PIN_PHASE_0, X_PIN_PHASE_1, X_PIN_PHASE_2, X_PIN_PHASE_3); motor_class_steps = true; // could be moved to class - #else - #ifdef X_STEP_PIN - myMotor[X_AXIS][0] = new StandardStepper(X_AXIS, X_STEP_PIN, X_DIRECTION_PIN); - #else - myMotor[X_AXIS][0] = new Nullmotor(); - #endif - #endif +#elif defined(X_STEP_PIN) + myMotor[X_AXIS][0] = new StandardStepper(X_AXIS, X_STEP_PIN, X_DIRECTION_PIN); +#else + myMotor[X_AXIS][0] = new Nullmotor(); #endif #ifdef X2_TRINAMIC_DRIVER myMotor[X_AXIS][1] = new TrinamicDriver(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE, X2_CS_PIN, get_next_trinamic_driver_index()); +#elif defined(X2_SERVO) + myMotor[X_AXIS][1] = new RcServo(X2_AXIS, X2_SERVO_PIN); +#elif defined(X_UNIPOLAR) + myMotor[X_AXIS][1] = new UnipolarMotor(X2_AXIS, X2_PIN_PHASE_0, X2_PIN_PHASE_1, X2_PIN_PHASE_2, X2_PIN_PHASE_3); + motor_class_steps = true; // could be moved to class +#elif defined(X2_STEP_PIN) + myMotor[X_AXIS][1] = new StandardStepper(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN); #else - #ifdef X2_STEP_PIN - myMotor[X_AXIS][1] = new StandardStepper(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN); - #else - myMotor[X_AXIS][1] = new Nullmotor(); - #endif + myMotor[X_AXIS][1] = new Nullmotor(); #endif + + // this WILL be done better with settings #ifdef Y_TRINAMIC_DRIVER myMotor[Y_AXIS][0] = new TrinamicDriver(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE, Y_CS_PIN, get_next_trinamic_driver_index()); -#else - #ifdef Y_UNIPOLAR +#elif defined(Y_SERVO) + myMotor[Y_AXIS][0] = new RcServo(Y_AXIS, Y_SERVO_PIN); +#elif defined(Y_UNIPOLAR) myMotor[Y_AXIS][0] = new UnipolarMotor(Y_AXIS, Y_PIN_PHASE_0, Y_PIN_PHASE_1, Y_PIN_PHASE_2, Y_PIN_PHASE_3); motor_class_steps = true; // could be moved to class - #else - #ifdef Y_STEP_PIN - myMotor[Y_AXIS][0] = new StandardStepper(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN); - #else - myMotor[Y_AXIS][0] = new Nullmotor(); - #endif - #endif +#elif defined(Y_STEP_PIN) + myMotor[Y_AXIS][0] = new StandardStepper(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN); +#else + myMotor[Y_AXIS][0] = new Nullmotor(); #endif #ifdef Y2_TRINAMIC_DRIVER myMotor[Y_AXIS][1] = new TrinamicDriver(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE, Y2_CS_PIN, get_next_trinamic_driver_index()); +#elif defined(Y2_SERVO) + myMotor[Y_AXIS][1] = new RcServo(Y2_AXIS, Y2_SERVO_PIN); +#elif defined(Y2_UNIPOLAR) + myMotor[Y_AXIS][1] = new UnipolarMotor(Y2_AXIS, Y2_PIN_PHASE_0, Y2_PIN_PHASE_1, Y2_PIN_PHASE_2, Y2_PIN_PHASE_3); + motor_class_steps = true; // could be moved to class +#elif defined(Y2_STEP_PIN) + myMotor[Y_AXIS][1] = new StandardStepper(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN); #else - #ifdef Y2_STEP_PIN - myMotor[Y_AXIS][1] = new StandardStepper(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN); - #else - myMotor[Y_AXIS][1] = new Nullmotor(); - #endif + myMotor[Y_AXIS][1] = new Nullmotor(); #endif + // this WILL be done better with settings #ifdef Z_TRINAMIC_DRIVER myMotor[Z_AXIS][0] = new TrinamicDriver(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE, Z_CS_PIN, get_next_trinamic_driver_index()); +#elif defined(Z_SERVO) + myMotor[Z_AXIS][0] = new RcServo(Z_AXIS, Z_SERVO_PIN); +#elif defined(Z_UNIPOLAR) + myMotor[Z_AXIS][0] = new UnipolarMotor(Z_AXIS, Z_PIN_PHASE_0, Z_PIN_PHASE_1, Z_PIN_PHASE_2, Z_PIN_PHASE_3); + motor_class_steps = true; // could be moved to class +#elif defined(Z_STEP_PIN) + myMotor[Z_AXIS][0] = new StandardStepper(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN); #else - #ifdef Z_STEP_PIN - myMotor[Z_AXIS][0] = new StandardStepper(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN); - #else - myMotor[Z_AXIS][0] = new Nullmotor(); - #endif + myMotor[Z_AXIS][0] = new Nullmotor(); #endif #ifdef Z2_TRINAMIC_DRIVER myMotor[Z_AXIS][1] = new TrinamicDriver(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE, Z2_CS_PIN, get_next_trinamic_driver_index()); +#elif defined(Z2_SERVO) + myMotor[Z_AXIS][1] = new RcServo(Z2_AXIS, Z2_SERVO_PIN); +#elif defined(Z2_UNIPOLAR) + myMotor[Z_AXIS][1] = new UnipolarMotor(Z2_AXIS, Z2_PIN_PHASE_0, Z2_PIN_PHASE_1, Z2_PIN_PHASE_2, Z2_PIN_PHASE_3); + motor_class_steps = true; // could be moved to class +#elif defined(Z2_STEP_PIN) + myMotor[Z_AXIS][1] = new StandardStepper(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN); #else - #ifdef Z2_STEP_PIN - myMotor[Z_AXIS][1] = new StandardStepper(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN); - #else - myMotor[Z_AXIS][1] = new Nullmotor(); - #endif + myMotor[Z_AXIS][1] = new Nullmotor(); #endif + // this WILL be done better with settings #ifdef A_TRINAMIC_DRIVER myMotor[A_AXIS][0] = new TrinamicDriver(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_TRINAMIC_DRIVER, A_RSENSE, A_CS_PIN, get_next_trinamic_driver_index()); +#elif defined(A_SERVO) + myMotor[A_AXIS][0] = new RcServo(A_AXIS, A_SERVO_PIN); +#elif defined(A_UNIPOLAR) + myMotor[A_AXIS][0] = new UnipolarMotor(A_AXIS, A_PIN_PHASE_0, A_PIN_PHASE_1, A_PIN_PHASE_2, A_PIN_PHASE_3); + motor_class_steps = true; // could be moved to class +#elif defined(A_STEP_PIN) + myMotor[A_AXIS][0] = new StandardStepper(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN); #else - #ifdef A_STEP_PIN - myMotor[A_AXIS][0] = new StandardStepper(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN); - #else - myMotor[A_AXIS][0] = new Nullmotor(); - #endif + myMotor[A_AXIS][0] = new Nullmotor(); #endif #ifdef A2_TRINAMIC_DRIVER - myMotor[A_AXIS][1] = new TrinamicDriver(A_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE, A2_CS_PIN, get_next_trinamic_driver_index()); + myMotor[A_AXIS][1] = new TrinamicDriver(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE, A2_CS_PIN, get_next_trinamic_driver_index()); +#elif defined(A2_SERVO) + myMotor[A_AXIS][1] = new RcServo(A2_AXIS, A2_SERVO_PIN); +#elif defined(A2_UNIPOLAR) + myMotor[A_AXIS][1] = new UnipolarMotor(A2_AXIS, A2_PIN_PHASE_0, A2_PIN_PHASE_1, A2_PIN_PHASE_2, A2_PIN_PHASE_3); + motor_class_steps = true; // could be moved to class +#elif defined(A2_STEP_PIN) + myMotor[A_AXIS][1] = new StandardStepper(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN); #else - #ifdef A2_STEP_PIN - myMotor[A_AXIS][1] = new StandardStepper(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN); - #else - myMotor[A_AXIS][1] = new Nullmotor(); - #endif + myMotor[A_AXIS][1] = new Nullmotor(); #endif + // this WILL be done better with settings #ifdef B_TRINAMIC_DRIVER myMotor[B_AXIS][0] = new TrinamicDriver(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_TRINAMIC_DRIVER, B_RSENSE, B_CS_PIN, get_next_trinamic_driver_index()); +#elif defined(B_SERVO) + myMotor[B_AXIS][0] = new RcServo(B_AXIS, B_SERVO_PIN); +#elif defined(B_UNIPOLAR) + myMotor[B_AXIS][0] = new UnipolarMotor(B_AXIS, B_PIN_PHASE_0, B_PIN_PHASE_1, B_PIN_PHASE_2, B_PIN_PHASE_3); + motor_class_steps = true; // could be moved to class +#elif defined(B_STEP_PIN) + myMotor[B_AXIS][0] = new StandardStepper(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN); #else - #ifdef B_STEP_PIN - myMotor[B_AXIS][0] = new StandardStepper(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN); - #else - myMotor[B_AXIS][0] = new Nullmotor(); - #endif -#endif -#ifdef B2_TRINAMIC_DRIVER - myMotor[B_AXIS][1] = new TrinamicDriver(B_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE, B2_CS_PIN, get_next_trinamic_driver_index()); -#else - #ifdef B2_STEP_PIN - myMotor[B_AXIS][1] = new StandardStepper(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN); - #else - myMotor[B_AXIS][1] = new Nullmotor(); - #endif + myMotor[B_AXIS][0] = new Nullmotor(); #endif +#ifdef B2_TRINAMIC_DRIVER + myMotor[B_AXIS][1] = new TrinamicDriver(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE, B2_CS_PIN, get_next_trinamic_driver_index()); +#elif defined(B2_SERVO) + myMotor[B_AXIS][1] = new RcServo(B2_AXIS, B2_SERVO_PIN); +#elif defined(B2_UNIPOLAR) + myMotor[B_AXIS][1] = new UnipolarMotor(B2_AXIS, B2_PIN_PHASE_0, B2_PIN_PHASE_1, B2_PIN_PHASE_2, B2_PIN_PHASE_3); + motor_class_steps = true; // could be moved to class +#elif defined(B2_STEP_PIN) + myMotor[B_AXIS][1] = new StandardStepper(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN); +#else + myMotor[B_AXIS][1] = new Nullmotor(); +#endif + + // this WILL be done better with settings #ifdef C_TRINAMIC_DRIVER myMotor[C_AXIS][0] = new TrinamicDriver(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_TRINAMIC_DRIVER, C_RSENSE, C_CS_PIN, get_next_trinamic_driver_index()); +#elif defined(C_SERVO) + myMotor[C_AXIS][0] = new RcServo(C_AXIS, C_SERVO_PIN); +#elif defined(C_UNIPOLAR) + myMotor[C_AXIS][0] = new UnipolarMotor(C_AXIS, C_PIN_PHASE_0, C_PIN_PHASE_1, C_PIN_PHASE_2, C_PIN_PHASE_3); + motor_class_steps = true; // could be moved to class +#elif defined(C_STEP_PIN) + myMotor[C_AXIS][0] = new StandardStepper(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN); #else - #ifdef C_STEP_PIN - myMotor[C_AXIS][0] = new StandardStepper(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN); - #else - myMotor[C_AXIS][0] = new Nullmotor(); - #endif + myMotor[C_AXIS][0] = new Nullmotor(); #endif + #ifdef C2_TRINAMIC_DRIVER - myMotor[C_AXIS][1] = new TrinamicDriver(C_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE, C2_CS_PIN, get_next_trinamic_driver_index()); + myMotor[C_AXIS][1] = new TrinamicDriver(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE, C2_CS_PIN, get_next_trinamic_driver_index()); +#elif defined(C2_SERVO) + myMotor[C_AXIS][1] = new RcServo(C2_AXIS, C2_SERVO_PIN); +#elif defined(C2_UNIPOLAR) + myMotor[C_AXIS][1] = new UnipolarMotor(C2_AXIS, C2_PIN_PHASE_0, C2_PIN_PHASE_1, C2_PIN_PHASE_2, C2_PIN_PHASE_3); + motor_class_steps = true; // could be moved to class +#elif defined(C2_STEP_PIN) + myMotor[C_AXIS][1] = new StandardStepper(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN); #else - #ifdef C2_STEP_PIN - myMotor[C_AXIS][1] = new StandardStepper(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN); - #else - myMotor[C_AXIS][1] = new Nullmotor(); - #endif + myMotor[C_AXIS][1] = new Nullmotor(); #endif for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) { diff --git a/Grbl_Esp32/Motors/MotorClass.h b/Grbl_Esp32/Motors/MotorClass.h index 1927ac56..77dfa94c 100644 --- a/Grbl_Esp32/Motors/MotorClass.h +++ b/Grbl_Esp32/Motors/MotorClass.h @@ -98,12 +98,8 @@ class Motor { uint8_t dual_axis_index; // 0 = primary 1=ganged uint8_t is_active = false; - - - bool _is_homing; - - char _axis_name[10]; + char _axis_name[10];// this the name to use when reporting like "X" or "X2" }; @@ -172,6 +168,22 @@ class UnipolarMotor : public Motor { bool _enabled; }; +class RcServo : public Motor { + public: + RcServo(); + RcServo(uint8_t axis_index, uint8_t pwm_pin); + void config_message(); + void init(); + void _write_pwm(uint32_t duty); + void set_disable(bool disable); + + uint8_t _pwm_pin; + uint8_t _channel_num; + uint32_t _current_pwm_duty; + + void set_location(); +}; + diff --git a/Grbl_Esp32/Motors/RcServoClass.cpp b/Grbl_Esp32/Motors/RcServoClass.cpp new file mode 100644 index 00000000..8f063a96 --- /dev/null +++ b/Grbl_Esp32/Motors/RcServoClass.cpp @@ -0,0 +1,98 @@ +/* + StandardStepperClass.cpp + + This is used for a stepper motor that just requires step and direction + pins. + + TODO: Add an enable pin + + Part of Grbl_ESP32 + 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 . + +*/ + +// this is the pulse range of a the servo. Typical servos are 0.001 to 0.002 seconds +// some servos have a wider range. You can adjust this here or in the calibration feature +#define SERVO_MIN_PULSE_SEC 0.001 // min pulse in seconds +#define SERVO_MAX_PULSE_SEC 0.002 // max pulse in seconds + +#define SERVO_POSITION_MIN_DEFAULT 0.0 // mm +#define SERVO_POSITION_MAX_DEFAULT 20.0 // mm + +#define SERVO_PULSE_FREQ 50 // 50Hz ...This is a standard analog servo value. Digital ones can repeat faster + +#define SERVO_PULSE_RES_BITS 16 // bits of resolution of PWM (16 is max) +#define SERVO_PULSE_RES_COUNT 65535 // see above TODO...do the math here 2^SERVO_PULSE_RES_BITS + +#define SERVO_TIME_PER_BIT ((1.0 / (float)SERVO_PULSE_FREQ) / ((float)SERVO_PULSE_RES_COUNT) ) // seconds + +#define SERVO_MIN_PULSE (uint16_t)(SERVO_MIN_PULSE_SEC / SERVO_TIME_PER_BIT) // in timer counts +#define SERVO_MAX_PULSE (uint16_t)(SERVO_MAX_PULSE_SEC / SERVO_TIME_PER_BIT) // in timer counts + +#define SERVO_PULSE_RANGE (SERVO_MAX_PULSE-SERVO_MIN_PULSE) + +#define SERVO_CAL_MIN 20.0 // Percent: the minimum allowable calibration value +#define SERVO_CAL_MAX 180.0 // Percent: the maximum allowable calibration value + +#define SERVO_TIMER_INT_FREQ 20.0 // Hz This is the task frequency + +#define SERVO_HOMING_OFF 0 // servo is off during homing +#define SERVO_HOMING_TARGET 1 // servo is send to a location during homing + +RcServo :: RcServo() { + +} + +RcServo :: RcServo(uint8_t axis_index, uint8_t pwm_pin) { + this->axis_index = axis_index % MAX_AXES; + this->dual_axis_index = axis_index < MAX_AXES ? 0 : 1; // 0 = primary 1 = ganged + this->_pwm_pin = pwm_pin; + set_axis_name(); + init(); + config_message(); + is_active = true; // as opposed to NullMotors, this is a real motor +} + +void RcServo :: init() { + _channel_num = sys_get_next_PWM_chan_num(); + ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS); + ledcAttachPin(_pwm_pin, _channel_num); + _current_pwm_duty = 0; + +} + +void RcServo :: config_message() { + grbl_msg_sendf(CLIENT_SERIAL, + MSG_LEVEL_INFO, + "%s Axis RC Servo motor PWM:%d", + _axis_name, + _pwm_pin); +} + +void RcServo::_write_pwm(uint32_t duty) { + // to prevent excessive calls to ledcWrite, make sure duty hass changed + if (duty == _current_pwm_duty) + return; + + _current_pwm_duty = duty; + + ledcWrite(_channel_num, duty); +} + +// sets the PWM to zero. This allows most servos to be manually moved +void RcServo::set_disable(bool disable) { + if (disable) + _write_pwm(0); +} + diff --git a/Grbl_Esp32/Motors/StandardStepperClass.cpp b/Grbl_Esp32/Motors/StandardStepperClass.cpp index de346afe..ad88da9b 100644 --- a/Grbl_Esp32/Motors/StandardStepperClass.cpp +++ b/Grbl_Esp32/Motors/StandardStepperClass.cpp @@ -31,7 +31,7 @@ StandardStepper :: StandardStepper(uint8_t axis_index, gpio_num_t step_pin, uint this->dual_axis_index = axis_index < MAX_AXES ? 0 : 1; // 0 = primary 1 = ganged this->step_pin = step_pin; this->dir_pin = dir_pin; - + set_axis_name(); init(); config_message(); } @@ -87,20 +87,11 @@ void StandardStepper :: init_step_dir_pins() { } -void StandardStepper :: config_message() { - char ganged[3]; - - if (dual_axis_index) { - strcpy(ganged, "2"); - } else { - strcpy(ganged, ""); - } - +void StandardStepper :: config_message() { grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, - "%c%s Axis standard stepper motor Step:%d Dir:%d", - report_get_axis_letter(axis_index), - ganged, + "%s Axis standard stepper motor Step:%d Dir:%d", + _axis_name, step_pin, dir_pin); } diff --git a/Grbl_Esp32/Motors/TrinamicDriverClass.cpp b/Grbl_Esp32/Motors/TrinamicDriverClass.cpp index 01082b18..322194ab 100644 --- a/Grbl_Esp32/Motors/TrinamicDriverClass.cpp +++ b/Grbl_Esp32/Motors/TrinamicDriverClass.cpp @@ -42,6 +42,7 @@ TrinamicDriver :: TrinamicDriver( uint8_t axis_index, } void TrinamicDriver :: init() { + set_axis_name(); if (_driver_part_number == 2130) tmcstepper = new TMC2130Stepper(cs_pin, _r_sense, spi_index); else if (_driver_part_number == 5160) @@ -50,7 +51,7 @@ void TrinamicDriver :: init() { grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Trinamic unsupported p/n:%d", _driver_part_number); return; } - set_axis_name(); + config_message(); // TODO step pins init_step_dir_pins(); // from StandardStepper diff --git a/Grbl_Esp32/Motors/UnipolarMotorClass.cpp b/Grbl_Esp32/Motors/UnipolarMotorClass.cpp index d60290f3..065beab5 100644 --- a/Grbl_Esp32/Motors/UnipolarMotorClass.cpp +++ b/Grbl_Esp32/Motors/UnipolarMotorClass.cpp @@ -13,6 +13,7 @@ UnipolarMotor :: UnipolarMotor(uint8_t axis_index, uint8_t pin_phase0, uint8_t p _half_step = true; // TODO read from settings ... microstep > 1 = half step + set_axis_name(); init(); config_message(); } @@ -28,8 +29,8 @@ void UnipolarMotor :: init() { void UnipolarMotor :: config_message() { grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, - "%c Axis unipolar stepper motor Ph0:%d Ph1:%d Ph2:%d Ph3:%d", - report_get_axis_letter(axis_index), + "%s Axis unipolar stepper motor Ph0:%d Ph1:%d Ph2:%d Ph3:%d", + _axis_name, _pin_phase0, _pin_phase1, _pin_phase2,