mirror of
https://github.com/bdring/Grbl_Esp32.git
synced 2025-09-01 18:32:37 +02:00
WIP
This commit is contained in:
bdring
@@ -24,10 +24,10 @@
|
||||
The arm 0 values (angle) are the arms at horizontal.
|
||||
Positive angles are below horizontal.
|
||||
The machine's Z zero point in the kinematics is parallel to the arm axes.
|
||||
Delta_z_offset is the offset to the end effector joints at arm angle zero.
|
||||
The is calculated at startup and used in the forward kinematics
|
||||
Delta_z_offset is the Z distance from the arm axes 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
|
||||
Feedrate in gcode is in the cartesian units. This must be converted to the
|
||||
angles. This is done by calculating the segment move distance and the angle
|
||||
move distance and applying that ration to the feedrate.
|
||||
|
||||
@@ -87,9 +87,10 @@ void machine_init() {
|
||||
float cartesian[N_AXIS] = { 0.0, 0.0, 0.0 };
|
||||
|
||||
calc_forward_kinematics(angles, cartesian); // Sets the cartesian values
|
||||
delta_z_offset = cartesian[Z_AXIS];
|
||||
|
||||
// print a startup message to show the kinematics are enables
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]);
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, delta_z_offset);
|
||||
}
|
||||
|
||||
bool user_defined_homing() { // true = do not continue with normal Grbl homing
|
||||
@@ -106,13 +107,36 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
|
||||
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
|
||||
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
|
||||
float pos_cart[N_AXIS];
|
||||
KinematicError status;
|
||||
|
||||
// convert the current position to cartesian
|
||||
|
||||
calc_forward_kinematics(position, pos_cart);
|
||||
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "From Angs: %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "From Cart: %3.3f %3.3f %3.3f", pos_cart[0], pos_cart[1], pos_cart[2]);
|
||||
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Raw Target: %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
|
||||
grbl_msg_sendf(CLIENT_SERIAL,
|
||||
MsgLevel::Info,
|
||||
"Offsets: %3.3f %3.3f %3.3f",
|
||||
delta_z_offset,
|
||||
gc_state.coord_offset[Z_AXIS],
|
||||
gc_state.coord_system[Z_AXIS]);
|
||||
|
||||
//target[Z_AXIS] += delta_z_offset + (gc_state.coord_system[Z_AXIS] + gc_state.coord_offset[Z_AXIS]);
|
||||
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "To: %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
|
||||
status = delta_calcInverse(target, motor_angles);
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Angles: %3.3f %3.3f %3.3f", motor_angles[0], motor_angles[1], motor_angles[2]);
|
||||
|
||||
// see if start is in work area...if not skip segments and try to go to target
|
||||
KinematicError status = delta_calcInverse(position, motor_angles);
|
||||
status = delta_calcInverse(position, motor_angles);
|
||||
|
||||
if (status == KinematicError::OUT_OF_RANGE) {
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error");
|
||||
@@ -127,6 +151,14 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
|
||||
return;
|
||||
}
|
||||
|
||||
target[Z_AXIS] -= delta_z_offset; // restore it
|
||||
memcpy(position, target, sizeof(target));
|
||||
|
||||
memcpy(last_angle, motor_angles, sizeof(motor_angles));
|
||||
|
||||
mc_line(motor_angles, pl_data);
|
||||
return;
|
||||
|
||||
position[X_AXIS] += gc_state.coord_offset[X_AXIS];
|
||||
position[Y_AXIS] += gc_state.coord_offset[Y_AXIS];
|
||||
position[Z_AXIS] += gc_state.coord_offset[Z_AXIS];
|
||||
@@ -324,6 +356,18 @@ void kinematics_post_homing() {
|
||||
#ifdef USE_CUSTOM_HOMING
|
||||
|
||||
#else
|
||||
//sys_position[Z_AXIS] = delta_z_offset * axis_settings[Z_AXIS]->steps_per_mm->get();
|
||||
//plan_sync_position();
|
||||
last_angle[X_AXIS] = sys_position[X_AXIS] / axis_settings[X_AXIS]->steps_per_mm->get();
|
||||
last_angle[Y_AXIS] = sys_position[Y_AXIS] / axis_settings[Y_AXIS]->steps_per_mm->get();
|
||||
last_angle[Z_AXIS] = sys_position[Z_AXIS] / axis_settings[Z_AXIS]->steps_per_mm->get();
|
||||
|
||||
grbl_msg_sendf(CLIENT_SERIAL,
|
||||
MsgLevel::Info,
|
||||
"kinematics_post_homing: %3.3f, %3.3f, %3.3f",
|
||||
last_angle[X_AXIS],
|
||||
last_angle[Y_AXIS],
|
||||
last_angle[Z_AXIS]);
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -96,11 +96,11 @@ const int MAX_N_AXIS = 6;
|
||||
//#define CONNECT_TO_SSID "your SSID"
|
||||
//#define SSID_PASSWORD "your SSID password"
|
||||
//CONFIGURE_EYECATCH_BEGIN (DO NOT MODIFY THIS LINE)
|
||||
#define ENABLE_BLUETOOTH // enable bluetooth
|
||||
//#define ENABLE_BLUETOOTH // enable bluetooth
|
||||
|
||||
#define ENABLE_SD_CARD // enable use of SD Card to run jobs
|
||||
|
||||
#define ENABLE_WIFI //enable wifi
|
||||
//#define ENABLE_WIFI //enable wifi
|
||||
|
||||
#if defined(ENABLE_WIFI) || defined(ENABLE_BLUETOOTH)
|
||||
# define WIFI_OR_BLUETOOTH
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
#pragma once
|
||||
// clang-format off
|
||||
|
||||
/*
|
||||
tapster_pro.h
|
||||
@@ -37,115 +38,106 @@
|
||||
|
||||
#define HOMING_CURRENT_REDUCTION 1.0
|
||||
|
||||
|
||||
*/
|
||||
|
||||
#define N_AXIS 3
|
||||
|
||||
#define USE_KINEMATICS // there are kinematic equations for this machine
|
||||
#define USE_FWD_KINEMATICS // report in cartesian
|
||||
#define USE_MACHINE_INIT // There is some custom initialization for this machine
|
||||
|
||||
// ================== Delta Geometry ===========================
|
||||
|
||||
#define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks)
|
||||
#define RADIUS_EFF 220.0f // radius of end effector side (length of linkages)
|
||||
#define LENGTH_FIXED_SIDE 294.449f // sized of fixed side triangel
|
||||
#define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle
|
||||
#define SEGMENT_LENGTH 0.5f // segment length in mm
|
||||
#define MAX_NEGATIVE_ANGLE -45.0f //
|
||||
#define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks)
|
||||
#define RADIUS_EFF 220.0f // radius of end effector side (length of linkages)
|
||||
#define LENGTH_FIXED_SIDE 294.449f // sized of fixed side triangel
|
||||
#define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle
|
||||
#define SEGMENT_LENGTH 0.5f // segment length in mm
|
||||
#define MAX_NEGATIVE_ANGLE -0.75f //
|
||||
|
||||
// ================== Config ======================
|
||||
|
||||
#ifdef HOMING_CYCLE_0
|
||||
# undef HOMING_CYCLE_0
|
||||
#endif
|
||||
#define HOMING_CYCLE_0 ((1 << X_AXIS) | (1 << Y_AXIS) | (1 << Z_AXIS))
|
||||
|
||||
#ifdef HOMING_CYCLE_1
|
||||
# undef HOMING_CYCLE_1
|
||||
#endif
|
||||
|
||||
#ifdef HOMING_CYCLE_2
|
||||
# undef HOMING_CYCLE_2
|
||||
#endif
|
||||
// Set $Homing/Cycle0=XYZ
|
||||
|
||||
// ================== CPU MAP ======================
|
||||
|
||||
#define TRINAMIC_DAISY_CHAIN
|
||||
|
||||
#define TRINAMIC_RUN_MODE TrinamicMode ::StealthChop
|
||||
#define TRINAMIC_HOMING_MODE TrinamicMode ::StealthChop
|
||||
#define TRINAMIC_RUN_MODE TrinamicMode ::StealthChop
|
||||
#define TRINAMIC_HOMING_MODE TrinamicMode ::StealthChop
|
||||
|
||||
// Use SPI enable instead of the enable pin
|
||||
// The hardware enable pin is tied to ground
|
||||
#define USE_TRINAMIC_ENABLE
|
||||
|
||||
#define X_TRINAMIC_DRIVER 2130 // Which Driver Type?
|
||||
#define X_RSENSE TMC2130_RSENSE_DEFAULT
|
||||
#define X_STEP_PIN GPIO_NUM_12
|
||||
#define X_DIRECTION_PIN GPIO_NUM_14
|
||||
#define X_CS_PIN GPIO_NUM_17 // Daisy Chain, all share same CS pin
|
||||
#define X_TRINAMIC_DRIVER 2130 // Which Driver Type?
|
||||
#define X_RSENSE TMC2130_RSENSE_DEFAULT
|
||||
#define X_STEP_PIN GPIO_NUM_12
|
||||
#define X_DIRECTION_PIN GPIO_NUM_14
|
||||
#define X_CS_PIN GPIO_NUM_17 // Daisy Chain, all share same CS pin
|
||||
|
||||
#define Y_TRINAMIC_DRIVER 2130 // Which Driver Type?
|
||||
#define Y_RSENSE TMC2130_RSENSE_DEFAULT
|
||||
#define Y_STEP_PIN GPIO_NUM_27
|
||||
#define Y_DIRECTION_PIN GPIO_NUM_26
|
||||
#define Y_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin
|
||||
#define Y_TRINAMIC_DRIVER 2130 // Which Driver Type?
|
||||
#define Y_RSENSE TMC2130_RSENSE_DEFAULT
|
||||
#define Y_STEP_PIN GPIO_NUM_27
|
||||
#define Y_DIRECTION_PIN GPIO_NUM_26
|
||||
#define Y_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin
|
||||
|
||||
#define Z_TRINAMIC_DRIVER 2130 // Which Driver Type?
|
||||
#define Z_RSENSE TMC2130_RSENSE_DEFAULT
|
||||
#define Z_STEP_PIN GPIO_NUM_15
|
||||
#define Z_DIRECTION_PIN GPIO_NUM_2
|
||||
#define Z_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin
|
||||
#define Z_TRINAMIC_DRIVER 2130 // Which Driver Type?
|
||||
#define Z_RSENSE TMC2130_RSENSE_DEFAULT
|
||||
#define Z_STEP_PIN GPIO_NUM_15
|
||||
#define Z_DIRECTION_PIN GPIO_NUM_2
|
||||
#define Z_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin
|
||||
|
||||
#define X_LIMIT_PIN GPIO_NUM_36
|
||||
#define Y_LIMIT_PIN GPIO_NUM_39
|
||||
#define Z_LIMIT_PIN GPIO_NUM_34
|
||||
#define X_LIMIT_PIN GPIO_NUM_36
|
||||
#define Y_LIMIT_PIN GPIO_NUM_39
|
||||
#define Z_LIMIT_PIN GPIO_NUM_34
|
||||
|
||||
// ================= defaults ===========================
|
||||
|
||||
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // keep them on, the trinamics will reduce power at idle
|
||||
|
||||
#define DEFAULT_X_MICROSTEPS 8
|
||||
#define DEFAULT_Y_MICROSTEPS DEFAULT_X_MICROSTEPS
|
||||
#define DEFAULT_Z_MICROSTEPS DEFAULT_X_MICROSTEPS
|
||||
#define DEFAULT_X_MICROSTEPS 8
|
||||
#define DEFAULT_Y_MICROSTEPS DEFAULT_X_MICROSTEPS
|
||||
#define DEFAULT_Z_MICROSTEPS DEFAULT_X_MICROSTEPS
|
||||
|
||||
// some math to figure out microsteps per unit // units could bedegrees or radians
|
||||
#define UNITS_PER_REV (2.0 * M_PI) // 360.0 degrees or 6.2831853 radians
|
||||
#define STEPS_PER_REV 400.0
|
||||
#define REDUCTION_RATIO (60.0 / 16.0) // the pulleys on arm and motor
|
||||
#define MICROSTEPS_PER_REV (STEPS_PER_REV * (float)DEFAULT_X_MICROSTEPS * REDUCTION_RATIO)
|
||||
#define UNITS_PER_REV (2.0 * M_PI) // 360.0 degrees or 6.2831853 radians
|
||||
#define STEPS_PER_REV 400.0
|
||||
#define REDUCTION_RATIO (60.0 / 16.0) // the pulleys on arm and motor
|
||||
#define MICROSTEPS_PER_REV (STEPS_PER_REV * (float)DEFAULT_X_MICROSTEPS * REDUCTION_RATIO)
|
||||
|
||||
#define DEFAULT_X_STEPS_PER_MM (MICROSTEPS_PER_REV / UNITS_PER_REV)
|
||||
#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_STEPS_PER_MM (MICROSTEPS_PER_REV / UNITS_PER_REV)
|
||||
#define DEFAULT_Y_STEPS_PER_MM DEFAULT_X_STEPS_PER_MM
|
||||
#define DEFAULT_Z_STEPS_PER_MM DEFAULT_X_STEPS_PER_MM
|
||||
|
||||
#define DEFAULT_X_MAX_RATE 400.0 // mm/min
|
||||
#define DEFAULT_Y_MAX_RATE DEFAULT_X_MAX_RATE
|
||||
#define DEFAULT_Z_MAX_RATE DEFAULT_X_MAX_RATE
|
||||
#define DEFAULT_X_MAX_RATE 100.0 // mm/min
|
||||
#define DEFAULT_Y_MAX_RATE DEFAULT_X_MAX_RATE
|
||||
#define DEFAULT_Z_MAX_RATE DEFAULT_X_MAX_RATE
|
||||
|
||||
#define DEFAULT_X_ACCELERATION 90.0
|
||||
#define DEFAULT_Y_ACCELERATION DEFAULT_X_ACCELERATION
|
||||
#define DEFAULT_Z_ACCELERATION DEFAULT_X_ACCELERATION
|
||||
#define DEFAULT_X_ACCELERATION 20.0
|
||||
#define DEFAULT_Y_ACCELERATION DEFAULT_X_ACCELERATION
|
||||
#define DEFAULT_Z_ACCELERATION DEFAULT_X_ACCELERATION
|
||||
|
||||
// homing
|
||||
#define DEFAULT_HOMING_FEED_RATE 25
|
||||
#define DEFAULT_HOMING_SEEK_RATE 50
|
||||
#define DEFAULT_HOMING_DIR_MASK 7
|
||||
#define DEFAULT_HOMING_ENABLE 1
|
||||
#define DEFAULT_INVERT_LIMIT_PINS 0
|
||||
#define DEFAULT_HOMING_FEED_RATE 25
|
||||
#define DEFAULT_HOMING_SEEK_RATE 100
|
||||
#define DEFAULT_HOMING_DIR_MASK (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) // all axes home negative
|
||||
#define DEFAULT_HOMING_ENABLE 1
|
||||
#define DEFAULT_INVERT_LIMIT_PINS 0
|
||||
|
||||
// homing
|
||||
// The machine homes up and above center. MPos is the axis angle in radians
|
||||
// at the homing posiiton
|
||||
|
||||
#define DEFAULT_USER_INT_80 800 // $80 User integer setting
|
||||
#define DEFAULT_USER_INT_81 350 // $80 User integer setting
|
||||
#define DEFAULT_X_HOMING_MPOS -0.75 // neagtive because above horizontal
|
||||
#define DEFAULT_Y_HOMING_MPOS -0.75
|
||||
#define DEFAULT_Z_HOMING_MPOS -0.75
|
||||
|
||||
#define DEFAULT_X_MAX_TRAVEL 1.75 // 100 dgrees in radians
|
||||
#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL
|
||||
#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL
|
||||
// the total travel is straight down from horizontal (pi/2) + the up travel
|
||||
#define DEFAULT_X_MAX_TRAVEL ((M_PI / 2.0) - DEFAULT_X_HOMING_MPOS)
|
||||
#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL
|
||||
#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL
|
||||
|
||||
#define DEFAULT_X_HOMING_MPOS 0.75
|
||||
#define DEFAULT_Y_HOMING_MPOS 0.75
|
||||
#define DEFAULT_Z_HOMING_MPOS 0.75
|
||||
#define DEFAULT_HOMING_PULLOFF -DEFAULT_X_HOMING_MPOS
|
||||
|
||||
#define DEFAULT_X_CURRENT 1.0
|
||||
#define DEFAULT_Y_CURRENT DEFAULT_X_CURRENT
|
||||
@@ -155,4 +147,4 @@
|
||||
#define DEFAULT_Y_HOLD_CURRENT DEFAULT_X_HOLD_CURRENT
|
||||
#define DEFAULT_Z_HOLD_CURRENT DEFAULT_X_HOLD_CURRENT
|
||||
|
||||
#define SPINDLE_TYPE SpindleType::NONE
|
||||
#define SPINDLE_TYPE SpindleType::NONE
|
||||
@@ -37,6 +37,8 @@
|
||||
|
||||
#define MACHINE_NAME "Test Drive - Demo Only No I/O!"
|
||||
|
||||
#define N_AXIS 3
|
||||
|
||||
#define SPINDLE_TYPE SpindleType::NONE
|
||||
|
||||
#ifdef USE_RMT_STEPS
|
||||
|
||||
@@ -345,8 +345,8 @@ void mc_homing_cycle(uint8_t cycle_mask) {
|
||||
// This give kinematics a chance to do something after normal homing
|
||||
kinematics_post_homing();
|
||||
#endif
|
||||
// If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
|
||||
limits_init();
|
||||
// If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
|
||||
limits_init();
|
||||
}
|
||||
|
||||
// Perform tool length probe cycle. Requires probe switch.
|
||||
|
||||
@@ -66,6 +66,7 @@ namespace Motors {
|
||||
tmcstepper->setSPISpeed(TRINAMIC_SPI_FREQ);
|
||||
}
|
||||
|
||||
read_settings(); // pull info from settings
|
||||
config_message();
|
||||
|
||||
// init() must be called later, after all TMC drivers have CS pins setup.
|
||||
@@ -163,6 +164,17 @@ namespace Motors {
|
||||
if (has_errors)
|
||||
return;
|
||||
|
||||
float max_travel = axis_settings[_axis_index]->max_travel->get();
|
||||
float mpos = axis_settings[_axis_index]->home_mpos->get();
|
||||
|
||||
if (bit_istrue(homing_dir_mask->get(), bit(_axis_index))) {
|
||||
_position_min = mpos;
|
||||
_position_max = mpos + max_travel;
|
||||
} else {
|
||||
_position_min = mpos - max_travel;
|
||||
_position_max = mpos;
|
||||
}
|
||||
|
||||
uint16_t run_i_ma = (uint16_t)(axis_settings[_axis_index]->run_current->get() * 1000.0);
|
||||
float hold_i_percent;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user