mirror/Grbl_Esp32
1
0
Fork 0
mirror of https://github.com/bdring/Grbl_Esp32.git synced 2025-09-02 19:02:35 +02:00
Code Issues Releases Wiki Activity

Finished RC Servo class

Browse Source 
- still needs a lot of cleanup and refactoring, but everything seems to work.
- All machine definition files except motor_class_test.h NEED to be updated to the new system.
This commit is contained in:
bdring
2020-05-14 13:27:45 -05:00
parent db088402d0
commit d295bf69bb
10 changed files with 351 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
Grbl_Esp32/Machines/motor_class_test.h
View File

@@ -214,8 +214,8 @@
*/
// ======================= SPI 4 axis xxyz ===========================
#define MACHINE_NAME "MotorClass Test 4x SPI XXYZ"
#define N_AXIS 3
#define MACHINE_NAME "MotorClass Test 4x SPI XXYZ A Servo"
#define N_AXIS 4
#define TRINAMIC_DAISY_CHAIN
@@ -244,14 +244,14 @@
#define Z_TRINAMIC_DRIVER 2130
#define Z_RSENSE TMC2130_RSENSE_DEFAULT
#define Z_STEP_PIN GPIO_NUM_33
#define Z_DIRECTION_PIN GPIO_NUM_32
#define Z_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin
#define Z_RSENSE TMC2130_RSENSE_DEFAULT
#define Z_STEP_PIN GPIO_NUM_33
#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
#define A_SERVO_PIN GPIO_NUM_25
#define A_SERVO_RANGE_MIN 0.0
#define A_SERVO_RANGE_MAX 5.0
#define SPINDLE_TYPE SPINDLE_TYPE_NONE

192
Grbl_Esp32/Motors/MotorClass.cpp
View File

@@ -16,10 +16,10 @@
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
TODO
TODO
Make sure public/private/protected is cleaned up.
Only a few Unipolar axes have been setup in init()
Deal with custom machine ... machine_trinamic_setup();
Class is ready to deal with non SPI pins, but they have not been needed yet.
It would be nice in the config message though
@@ -30,10 +30,10 @@
MPCNC (ganged with shared direction pin)
TMC2130 Pen Laser (trinamics, stallguard tuning)
Unipolar
TODO
4 Axis SPI (Daisy Chain, Ganged with unique direction pins)
Reference
TMC2130 Datasheet https://www.trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC2130_datasheet.pdf
@@ -45,6 +45,7 @@
Motor* myMotor[6][2]; // number of axes (normal and ganged)
static TaskHandle_t readSgTaskHandle = 0; // for realtime stallguard data diaplay
static TaskHandle_t servoUpdateTaskHandle = 0;
uint8_t rmt_chan_num[6][2];
rmt_item32_t rmtItem[2];
@@ -52,8 +53,9 @@ rmt_config_t rmtConfig;
bool motor_class_steps; // true if at least one motor class is handling steps
void init_motors() {
motor_class_steps = false;
void init_motors() {
bool need_servo_task = false;
motor_class_steps = false; // does any class need step info?
// TODO SPI needs to be done before constructors because they have an init that uses SPI
// Should move all inits to the end and conditionally turn on SPI
SPI.begin(); // Yes, I know about the SD issue
@@ -62,27 +64,25 @@ void init_motors() {
#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());
#elif defined(X_SERVO)
myMotor[X_AXIS][0] = new RcServo(X_AXIS, X_SERVO_PIN);
myMotor[X_AXIS][0] = new RcServo(X_AXIS, X_SERVO_PIN, X_SERVO_RANGE_MIN, X_SERVO_RANGE_MAX);
#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
myMotor[X_AXIS][0] = new UnipolarMotor(X_AXIS, X_PIN_PHASE_0, X_PIN_PHASE_1, X_PIN_PHASE_2, X_PIN_PHASE_3);
#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();
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);
myMotor[X_AXIS][1] = new RcServo(X2_AXIS, X2_SERVO_PIN), X2_SERVO_RANGE_MIN, X2_SERVO_RANGE_MAX;
#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
myMotor[X_AXIS][1] = new UnipolarMotor(X2_AXIS, X2_PIN_PHASE_0, X2_PIN_PHASE_1, X2_PIN_PHASE_2, X2_PIN_PHASE_3);
#elif defined(X2_STEP_PIN)
myMotor[X_AXIS][1] = new StandardStepper(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN);
#else
myMotor[X_AXIS][1] = new Nullmotor();
myMotor[X_AXIS][1] = new Nullmotor();
#endif
@@ -90,23 +90,21 @@ void init_motors() {
#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());
#elif defined(Y_SERVO)
myMotor[Y_AXIS][0] = new RcServo(Y_AXIS, Y_SERVO_PIN);
myMotor[Y_AXIS][0] = new RcServo(Y_AXIS, Y_SERVO_PIN, Y_SERVO_RANGE_MIN, Y_SERVO_RANGE_MAX);
#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
myMotor[Y_AXIS][0] = new UnipolarMotor(Y_AXIS, Y_PIN_PHASE_0, Y_PIN_PHASE_1, Y_PIN_PHASE_2, Y_PIN_PHASE_3);
#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();
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);
myMotor[Y_AXIS][1] = new RcServo(Y2_AXIS, Y2_SERVO_PIN, Y2_SERVO_RANGE_MIN, Y2_SERVO_RANGE_MAX);
#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
myMotor[Y_AXIS][1] = new UnipolarMotor(Y2_AXIS, Y2_PIN_PHASE_0, Y2_PIN_PHASE_1, Y2_PIN_PHASE_2, Y2_PIN_PHASE_3);
#elif defined(Y2_STEP_PIN)
myMotor[Y_AXIS][1] = new StandardStepper(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN);
#else
@@ -117,124 +115,115 @@ void init_motors() {
#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);
myMotor[Z_AXIS][0] = new RcServo(Z_AXIS, Z_SERVO_PIN, Z_SERVO_RANGE_MIN, Z_SERVO_RANGE_MAX);
#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
myMotor[Z_AXIS][0] = new UnipolarMotor(Z_AXIS, Z_PIN_PHASE_0, Z_PIN_PHASE_1, Z_PIN_PHASE_2, Z_PIN_PHASE_3);
#elif defined(Z_STEP_PIN)
myMotor[Z_AXIS][0] = new StandardStepper(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN);
#else
myMotor[Z_AXIS][0] = new Nullmotor();
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);
myMotor[Z_AXIS][1] = new RcServo(Z2_AXIS, Z2_SERVO_PIN, Z2_SERVO_RANGE_MIN, Z2_SERVO_RANGE_MAX);
#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
myMotor[Z_AXIS][1] = new UnipolarMotor(Z2_AXIS, Z2_PIN_PHASE_0, Z2_PIN_PHASE_1, Z2_PIN_PHASE_2, Z2_PIN_PHASE_3);
#elif defined(Z2_STEP_PIN)
myMotor[Z_AXIS][1] = new StandardStepper(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN);
#else
myMotor[Z_AXIS][1] = new Nullmotor();
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);
myMotor[A_AXIS][0] = new RcServo(A_AXIS, A_SERVO_PIN, A_SERVO_RANGE_MIN, A_SERVO_RANGE_MAX);
#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
myMotor[A_AXIS][0] = new UnipolarMotor(A_AXIS, A_PIN_PHASE_0, A_PIN_PHASE_1, A_PIN_PHASE_2, A_PIN_PHASE_3);
#elif defined(A_STEP_PIN)
myMotor[A_AXIS][0] = new StandardStepper(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN);
#else
myMotor[A_AXIS][0] = new Nullmotor();
myMotor[A_AXIS][0] = new Nullmotor();
#endif
#ifdef A2_TRINAMIC_DRIVER
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);
myMotor[A_AXIS][1] = new RcServo(A2_AXIS, A2_SERVO_PIN, A2_SERVO_RANGE_MIN, A2_SERVO_RANGE_MAX);
#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
myMotor[A_AXIS][1] = new UnipolarMotor(A2_AXIS, A2_PIN_PHASE_0, A2_PIN_PHASE_1, A2_PIN_PHASE_2, A2_PIN_PHASE_3);
#elif defined(A2_STEP_PIN)
myMotor[A_AXIS][1] = new StandardStepper(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN);
#else
myMotor[A_AXIS][1] = new Nullmotor();
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);
myMotor[B_AXIS][0] = new RcServo(B_AXIS, B_SERVO_PIN, B_SERVO_RANGE_MIN, B_SERVO_RANGE_MAX);
#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
myMotor[B_AXIS][0] = new UnipolarMotor(B_AXIS, B_PIN_PHASE_0, B_PIN_PHASE_1, B_PIN_PHASE_2, B_PIN_PHASE_3);
#elif defined(B_STEP_PIN)
myMotor[B_AXIS][0] = new StandardStepper(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN);
#else
myMotor[B_AXIS][0] = new Nullmotor();
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);
myMotor[B_AXIS][1] = new RcServo(B2_AXIS, B2_SERVO_PIN, B2_SERVO_RANGE_MIN, B2_SERVO_RANGE_MAX);
#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
myMotor[B_AXIS][1] = new UnipolarMotor(B2_AXIS, B2_PIN_PHASE_0, B2_PIN_PHASE_1, B2_PIN_PHASE_2, B2_PIN_PHASE_3);
#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();
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);
myMotor[C_AXIS][0] = new RcServo(C_AXIS, C_SERVO_PIN, C_SERVO_RANGE_MIN, C_SERVO_RANGE_MAX);
#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
myMotor[C_AXIS][0] = new UnipolarMotor(C_AXIS, C_PIN_PHASE_0, C_PIN_PHASE_1, C_PIN_PHASE_2, C_PIN_PHASE_3);
#elif defined(C_STEP_PIN)
myMotor[C_AXIS][0] = new StandardStepper(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN);
#else
myMotor[C_AXIS][0] = new Nullmotor();
myMotor[C_AXIS][0] = new Nullmotor();
#endif
#ifdef C2_TRINAMIC_DRIVER
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);
myMotor[C_AXIS][1] = new RcServo(C2_AXIS, C2_SERVO_PIN, C2_SERVO_RANGE_MIN, C2_SERVO_RANGE_MAX);
#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
myMotor[C_AXIS][1] = new UnipolarMotor(C2_AXIS, C2_PIN_PHASE_0, C2_PIN_PHASE_1, C2_PIN_PHASE_2, C2_PIN_PHASE_3);
#elif defined(C2_STEP_PIN)
myMotor[C_AXIS][1] = new StandardStepper(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN);
#else
myMotor[C_AXIS][1] = new Nullmotor();
myMotor[C_AXIS][1] = new Nullmotor();
#endif
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
myMotor[axis][gang_index]->test();
}
for (uint8_t gang_index = 0; gang_index < 2; gang_index++)
myMotor[axis][gang_index]->test();
}
#ifdef STEPPERS_DISABLE_PIN
grbl_msg_sendf(CLIENT_SERIAL,
#ifdef STEPPERS_DISABLE_PIN
grbl_msg_sendf(CLIENT_SERIAL,
MSG_LEVEL_INFO,
"Global stepper enable pin:%d",
STEPPERS_DISABLE_PIN);
pinMode(STEPPERS_DISABLE_PIN, OUTPUT); // global motor enable pin
pinMode(STEPPERS_DISABLE_PIN, OUTPUT); // global motor enable pin
#endif
#endif
motors_set_disable(true);
// tuning gets turned on if this is defined and laser mode is on at boot time.
@@ -249,7 +238,26 @@ void init_motors() {
0 // core
);
}
#endif
#endif
// certain motors need features to be turned on. Check them here
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
if (myMotor[axis][gang_index]->type_id == RC_SERVO_MOTOR) need_servo_task = true;
if (myMotor[axis][gang_index]->type_id == UNIPOLAR_MOTOR) motor_class_steps = true;
}
}
if (need_servo_task) {
xTaskCreatePinnedToCore(servoUpdateTask, // task
"servoUpdateTask", // name for task
4096, // size of task stack
NULL, // parameters
1, // priority
&servoUpdateTaskHandle,
0 // core
);
}
}
@@ -289,29 +297,47 @@ void readSgTask(void* pvParameters) {
}
}
void servoUpdateTask(void* pvParameters) {
TickType_t xLastWakeTime;
const TickType_t xUpdate = SERVO_TIMER_INT_FREQ; // in ticks (typically ms)
xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time.
while (true) { // don't ever return from this or the task dies
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo update");
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
for (uint8_t gang_index = 0; gang_index < 2; gang_index++)
myMotor[axis][gang_index]->update();
}
vTaskDelayUntil(&xLastWakeTime, xUpdate);
}
}
void motors_set_disable(bool disable) {
static bool _current_disable = true;
if (disable == _current_disable)
return;
_current_disable = disable;
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "motors_set_disable(%d)", disable);
// now step through all the motors
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++)
myMotor[axis][gang_index]->set_disable(disable);
}
}
// global disable pin
#ifdef STEPPERS_DISABLE_PIN
if (bit_istrue(settings.flags, BITFLAG_INVERT_ST_ENABLE)) {
disable = !disable; // Apply pin invert.
}
digitalWrite(STEPPERS_DISABLE_PIN, disable);
#endif
#ifdef STEPPERS_DISABLE_PIN
if (bit_istrue(settings.flags, BITFLAG_INVERT_ST_ENABLE)) {
disable = !disable; // Apply pin invert.
}
digitalWrite(STEPPERS_DISABLE_PIN, disable);
#endif
}
/*
@@ -331,17 +357,15 @@ void motor_read_settings() {
void motors_set_homing_mode(bool is_homing) {
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "motors_set_homing_mode(%d)", is_homing);
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++)
myMotor[axis][gang_index]->set_homing_mode(is_homing);
}
}
}
void motors_set_direction_pins(uint8_t onMask) {
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++)
myMotor[axis][gang_index]->set_direction_pins(onMask);
}
}
}
@@ -349,20 +373,20 @@ void motors_set_direction_pins(uint8_t onMask) {
currently much of the step pulse I/O is done in stepper.cpp
Some classes like UnipolarMotorClass do it themselves.
This gives every class a notification of step.
Note: global variable (bool motor_class_steps) decides whether this
Note: global variable (bool motor_class_steps) decides whether this
is needed to be called.
*/
void motors_step(uint8_t step_mask, uint8_t dir_mask) {
for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) {
for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++)
myMotor[axis][gang_index]->step(step_mask, dir_mask);
}
}
}
// ============================== Class Methods ================================================
Motor :: Motor() {
type_id = MOTOR;
}
void Motor :: init() {
@@ -376,18 +400,10 @@ void Motor :: set_disable(bool disable) {}
void Motor :: set_direction_pins(uint8_t onMask) {}
void Motor :: step(uint8_t step_mask, uint8_t dir_mask) {}
bool Motor :: test() {return true;}; // true = OK
void Motor :: update() {}
void Motor :: set_axis_name() {
char ganged[3];
if (dual_axis_index) {
strcpy(ganged, "2");
} else {
strcpy(ganged, "");
}
sprintf(_axis_name, "%c%s", report_get_axis_letter(axis_index), ganged);
sprintf(_axis_name, "%c%s", report_get_axis_letter(axis_index), dual_axis_index ? "2" : "");
}
void Motor :: set_homing_mode(bool is_homing) {

43
Grbl_Esp32/Motors/MotorClass.h
View File

@@ -31,6 +31,8 @@
*/
/*
#define TRINAMIC_RUN_MODE_STEALTHCHOP 0 // very quiet
#define TRINAMIC_RUN_MODE_COOLSTEP 1 // everything runs cooler so higher current possible
#define TRINAMIC_RUN_MODE_STALLGUARD 2 // everything runs cooler so higher current possible
@@ -43,26 +45,30 @@
#define TMC2130_RSENSE_DEFAULT 0.11f
#define TMC5160_RSENSE_DEFAULT 0.075f
*/
// ============ defaults =================
#ifndef TRINAMIC_RUN_MODE
#define TRINAMIC_RUN_MODE TRINAMIC_RUN_MODE_COOLSTEP
#endif
#ifndef TRINAMIC_HOMING_MODE
#define TRINAMIC_HOMING_MODE TRINAMIC_HOMING_NONE
#endif
#ifndef MOTORCLASS_H
#define MOTORCLASS_H
#include "../grbl.h"
#include <TMCStepper.h> // https://github.com/teemuatlut/TMCStepper
#include "TrinamicDriverClass.h"
#include "RcServoClass.h"
extern uint8_t rmt_chan_num[MAX_AXES][2];
extern rmt_item32_t rmtItem[2];
extern rmt_config_t rmtConfig;
typedef enum {
MOTOR,
NULL_MOTOR,
STANDARD_MOTOR,
TRINAMIC_SPI_MOTOR,
UNIPOLAR_MOTOR,
RC_SERVO_MOTOR
} motor_class_id_t;
// ========== global functions ===================
// These are used for setup and to talk to the motors as a group.
@@ -75,6 +81,8 @@ void motors_set_disable(bool disable);
void motors_set_direction_pins(uint8_t onMask);
void motors_step(uint8_t step_mask, uint8_t dir_mask);
void servoUpdateTask(void* pvParameters);
extern bool motor_class_steps; // true if at least one motor class is handling steps
// ==================== Classes ====================
@@ -93,10 +101,14 @@ class Motor {
virtual void step(uint8_t step_mask, uint8_t dir_mask); // only used on Unipolar right now
virtual bool test();
virtual void set_axis_name();
virtual void update();
motor_class_id_t type_id;
uint8_t axis_index; // X_AXIS, etc
uint8_t dual_axis_index; // 0 = primary 1=ganged
uint8_t is_active = false;
bool _showError;
bool _use_mpos = true;
bool _is_homing;
char _axis_name[10];// this the name to use when reporting like "X" or "X2"
@@ -149,6 +161,7 @@ class TrinamicDriver : public StandardStepper {
uint32_t calc_tstep(float speed, float percent);
};
class UnipolarMotor : public Motor {
public:
UnipolarMotor();
@@ -171,20 +184,30 @@ class UnipolarMotor : public Motor {
class RcServo : public Motor {
public:
RcServo();
RcServo(uint8_t axis_index, uint8_t pwm_pin);
RcServo(uint8_t axis_index, uint8_t pwm_pin, float min, float max);
void config_message();
void init();
void _write_pwm(uint32_t duty);
void set_disable(bool disable);
void update();
void read_settings();
uint8_t _pwm_pin;
uint8_t _channel_num;
uint32_t _current_pwm_duty;
float _position_min;
float _position_max; // position in millimeters
float _homing_position;
float servo_pulse_min;
float servo_pulse_max;
void set_location();
void _get_calibration();
};
#endif
#endif

177
Grbl_Esp32/Motors/RcServoClass.cpp
View File

@@ -1,15 +1,39 @@
/*
StandardStepperClass.cpp
RcServo.cpp
This is used for a stepper motor that just requires step and direction
pins.
TODO: Add an enable pin
This allows an RcServo to be used like any other motor. Serrvos
do have limitation in travel and speed, so you do need to respect that.
Part of Grbl_ESP32
2020 - Bart Dring
Grbl is free software: you can redistribute it and/or modify
Servos have a limited travel, so they map the travel across a range in
the current work coordinatee system. The servo can only travel as far
as the range, but the internal axis value can keep going.
Range: The range is specified in the machine definition file with...
#define X_SERVO_RANGE_MIN 0.0
#define X_SERVO_RANGE_MAX 5.0
Direction: The direction can be changed using the $3 setting for the axis
Homing: During homing, the servo will move to one of the endpoints. The
endpoint is determined by the $23 setting for the axis. Do not define
a homing cycle for the axis with the servo.
You do need at least 1 homing cycle. TODO: Fix this
Calibration. You can tweak the endpoints using the $10n and $13n setting,
where n is the axis. The value is a percent. If you secify a percent outside the
the range specified by the values below, it will be reset to 100.0 (100% ... no change)
The calibration adjusts in direction of positive momement, so a value above 100% moves
towards the higher axis value.
#define SERVO_CAL_MIN
#define SERVO_CAL_MAX
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.
@@ -22,42 +46,17 @@
*/
// 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) {
RcServo :: RcServo(uint8_t axis_index, uint8_t pwm_pin, float min, float max) {
type_id = RC_SERVO_MOTOR;
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;
_position_min = min;
_position_max = max;
set_axis_name();
init();
config_message();
@@ -65,19 +64,25 @@ RcServo :: RcServo(uint8_t axis_index, uint8_t pwm_pin) {
}
void RcServo :: init() {
read_settings();
_channel_num = sys_get_next_PWM_chan_num();
ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS);
ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS);
ledcAttachPin(_pwm_pin, _channel_num);
_current_pwm_duty = 0;
pinMode(GPIO_NUM_4, OUTPUT);
digitalWrite(GPIO_NUM_4, HIGH);
}
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 :: config_message() {
grbl_msg_sendf(CLIENT_SERIAL,
MSG_LEVEL_INFO,
"%s Axis RC Servo motor Output:%d Min:%5.3fmm Max:%5.3fmm",
_axis_name,
_pwm_pin,
_position_min,
_position_max);
}
void RcServo::_write_pwm(uint32_t duty) {
@@ -86,7 +91,8 @@ void RcServo::_write_pwm(uint32_t duty) {
return;
_current_pwm_duty = duty;
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Servo Pwm %d", _axis_name, duty);
ledcWrite(_channel_num, duty);
}
@@ -96,3 +102,88 @@ void RcServo::set_disable(bool disable) {
_write_pwm(0);
}
void RcServo::update() {
set_location();
}
void RcServo::set_location() {
uint32_t servo_pulse_len;
float servo_pos, mpos, offset;
// skip location if we are in alarm mode
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "locate");
if (sys.state == STATE_ALARM) {
set_disable(true);
return;
}
if (sys.state == STATE_HOMING) {
if (bit_istrue(settings.homing_dir_mask, bit(axis_index))) {
servo_pos = _position_min; // go to servos home position
} else {
servo_pos = _position_max; // go to servos home position
}
} else {
mpos = system_convert_axis_steps_to_mpos(sys_position, axis_index); // get the axis machine position in mm
offset = gc_state.coord_system[axis_index] + gc_state.coord_offset[axis_index]; // get the current axis work offset
servo_pos = mpos - offset; // determine the current work position
}
// determine the pulse length
servo_pulse_len = (uint32_t)mapConstrain(servo_pos, _position_min, _position_max, servo_pulse_min, servo_pulse_max);
_write_pwm(servo_pulse_len);
}
void RcServo::read_settings() {
_get_calibration();
}
// this should change to use its own settings.
void RcServo::_get_calibration() {
bool settingsOK = true;
float _cal_min, _cal_max;
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Read settings");
// make sure the min is in range
if ((settings.steps_per_mm[axis_index] < SERVO_CAL_MIN) || (settings.steps_per_mm[axis_index] > SERVO_CAL_MAX)) {
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo calibration ($10%d) value error. Reset to 100", axis_index);
settings.steps_per_mm[axis_index] = 100;
settingsOK = false;
}
// make sure the max is in range
// Note: Max travel is set positive via $$, but stored as a negative number
if ((settings.max_travel[axis_index] < -SERVO_CAL_MAX) || (settings.max_travel[axis_index] > -SERVO_CAL_MIN)) {
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo calibration ($13%d) value error. Reset to 100", axis_index);
settings.max_travel[axis_index] = -100;
settingsOK = false;
}
if (! settingsOK) {
write_global_settings(); // they were changed, so write them
}
servo_pulse_min = SERVO_MIN_PULSE;
servo_pulse_max = SERVO_MAX_PULSE;
// apply inverts and store them in local variable
if (bit_istrue(settings.dir_invert_mask, bit(axis_index))) { // normal direction
_cal_min = 2.0 - (settings.steps_per_mm[axis_index] / 100.0);
_cal_max = 2.0 - (settings.max_travel[axis_index] / -100.0);
swap(servo_pulse_min, servo_pulse_max);
} else { // inverted direction
_cal_min = (settings.steps_per_mm[axis_index] / 100.0);
//_cal_max = 2.0 - (settings.max_travel[axis_index] / -100.0);
_cal_max = (settings.max_travel[axis_index] / -100.0);
}
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo calibration min%1.2f max %1.2f", _cal_min, _cal_max );
servo_pulse_min *= _cal_min;
servo_pulse_max *= _cal_max;
}

26
Grbl_Esp32/Motors/RcServoClass.h Normal file
View File

@@ -0,0 +1,26 @@
// 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 50.0 // Hz This is the task frequency

1
Grbl_Esp32/Motors/StandardStepperClass.cpp
View File

@@ -27,6 +27,7 @@ StandardStepper :: StandardStepper() {
}
StandardStepper :: StandardStepper(uint8_t axis_index, gpio_num_t step_pin, uint8_t dir_pin) {
type_id = STANDARD_MOTOR;
this->axis_index = axis_index % MAX_AXES;
this->dual_axis_index = axis_index < MAX_AXES ? 0 : 1; // 0 = primary 1 = ganged
this->step_pin = step_pin;

2
Grbl_Esp32/Motors/TrinamicDriverClass.cpp
View File

@@ -21,6 +21,7 @@
*/
#include <TMCStepper.h>
#include "TrinamicDriverClass.h"
#define TRINAMIC_FCLK 12700000.0 // Internal clock Approx (Hz) used to calculate TSTEP from homing rate
@@ -30,6 +31,7 @@ TrinamicDriver :: TrinamicDriver( uint8_t axis_index,
uint16_t driver_part_number,
float r_sense, uint8_t cs_pin,
int8_t spi_index) {
type_id = TRINAMIC_SPI_MOTOR;
this->axis_index = axis_index % MAX_AXES;
this->dual_axis_index = axis_index < 6 ? 0 : 1; // 0 = primary 1 = ganged
_driver_part_number = driver_part_number;

33
Grbl_Esp32/Motors/TrinamicDriverClass.h Normal file
View File

@@ -0,0 +1,33 @@
#define TRINAMIC_RUN_MODE_STEALTHCHOP 0 // very quiet
#define TRINAMIC_RUN_MODE_COOLSTEP 1 // everything runs cooler so higher current possible
#define TRINAMIC_RUN_MODE_STALLGUARD 2 // everything runs cooler so higher current possible
#define TRINAMIC_HOMING_NONE 0
#define TRINAMIC_HOMING_STALLGUARD 1
#define NORMAL_TCOOLTHRS 0xFFFFF // 20 bit is max
#define NORMAL_THIGH 0
#define TMC2130_RSENSE_DEFAULT 0.11f
#define TMC5160_RSENSE_DEFAULT 0.075f
// ============ defaults =================
#ifndef TRINAMIC_RUN_MODE
#define TRINAMIC_RUN_MODE TRINAMIC_RUN_MODE_COOLSTEP
#endif
#ifndef TRINAMIC_HOMING_MODE
#define TRINAMIC_HOMING_MODE TRINAMIC_HOMING_NONE
#endif
#ifndef TRINAMICDRIVERCLASS_H
#define TRINAMICDRIVERCLASS_H
#include "MotorClass.h"
#include <TMCStepper.h> // https://github.com/teemuatlut/TMCStepper
#endif

1
Grbl_Esp32/Motors/UnipolarMotorClass.cpp
View File

@@ -4,6 +4,7 @@ UnipolarMotor :: UnipolarMotor() {
UnipolarMotor :: UnipolarMotor(uint8_t axis_index, uint8_t pin_phase0, uint8_t pin_phase1, uint8_t pin_phase2, uint8_t pin_phase3) {
type_id = UNIPOLAR_MOTOR;
this->axis_index = axis_index % MAX_AXES;
this->dual_axis_index = axis_index < MAX_AXES ? 0 : 1; // 0 = primary 1 = ganged
_pin_phase0 = pin_phase0;

8
Grbl_Esp32/settings.cpp
View File

@@ -275,15 +275,20 @@ uint8_t settings_store_global_setting(uint8_t parameter, float value) {
if (value * settings.max_rate[parameter] > (MAX_STEP_RATE_HZ * 60.0)) return (STATUS_MAX_STEP_RATE_EXCEEDED);
#endif
settings.steps_per_mm[parameter] = value;
motor_read_settings();
break;
case 1:
#ifdef MAX_STEP_RATE_HZ
if (value * settings.steps_per_mm[parameter] > (MAX_STEP_RATE_HZ * 60.0)) return (STATUS_MAX_STEP_RATE_EXCEEDED);
#endif
settings.max_rate[parameter] = value;
motor_read_settings();
break;
case 2: settings.acceleration[parameter] = value * 60 * 60; break; // Convert to mm/min^2 for grbl internal use.
case 3: settings.max_travel[parameter] = -value; break; // Store as negative for grbl internal use.
case 3:
settings.max_travel[parameter] = -value;
motor_read_settings();
break; // Store as negative for grbl internal use.
case 4: // run current
settings.current[parameter] = value;
settings_spi_driver_init();
@@ -323,6 +328,7 @@ uint8_t settings_store_global_setting(uint8_t parameter, float value) {
case 3:
settings.dir_invert_mask = int_value;
st_generate_step_dir_invert_masks(); // Regenerate step and direction port invert masks.
motor_read_settings();
break;
case 4: // Reset to ensure change. Immediate re-init may cause problems.
if (int_value) settings.flags |= BITFLAG_INVERT_ST_ENABLE;