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

WIP

Browse Source
This commit is contained in:
bdring
2020-10-05 12:08:18 -05:00
parent 58e5059f8d
commit 0306ec4709
6 changed files with 86 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
Grbl_Esp32/Custom/parallel_delta.cpp
View File

@@ -7,6 +7,9 @@
Kinematics for a parallel delta robot.
Note: You must do a clean before compiling whenever this file is altered!
==================== How it Works ====================================
On a delta machine, Grbl axis units are in radians
@@ -24,8 +27,8 @@
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 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
The offset of the Z distance from the arm axes to the end effector joints
at arm angle zero will be printed at startup on the serial port.
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
@@ -33,6 +36,7 @@
TODO Cleanup
Update so extra axes get delt with ... passed through properly
Have MPos use kinematics too
============================================================================
@@ -56,11 +60,13 @@ enum class KinematicError : uint8_t {
NONE = 0,
OUT_OF_RANGE = 1,
ANGLE_TOO_NEGATIVE = 2,
ANGLE_TOO_POSITIVE = 3,
};
// Create custom run time $ settings
FloatSetting* kinematic_segment_len;
// trigonometric constants to speed calculations
// trigonometric constants to speed up calculations
const float sqrt3 = 1.732050807;
const float dtr = M_PI / (float)180.0; // degrees to radians
const float sin120 = sqrt3 / 2.0;
@@ -76,8 +82,9 @@ const float f = LENGTH_FIXED_SIDE; // sized of fixed side triangel
const float e = LENGTH_EFF_SIDE; // size of end effector side triangle
static float last_angle[3] = { 0.0, 0.0, 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs
static float last_cartesian[N_AXIS] = { 0.0, 0.0, 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs
float delta_z_offset; // Z offset of the effector from the arm centers
static float last_cartesian[N_AXIS] = {
0.0, 0.0, 0.0
}; // A place to save the previous motor angles for distance/feed rate calcs // Z offset of the effector from the arm centers
// prototypes for helper functions
int calc_forward_kinematics(float* angles, float* cartesian);
@@ -86,18 +93,18 @@ KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta);
float three_axis_dist(float* point1, float* point2);
void machine_init() {
// Calculate the Z offset at the motor zero angles ...
// Z offset is the z distance from the motor axes to the end effector axes at zero angle
float angles[N_AXIS] = { 0.0, 0.0, 0.0 };
float cartesian[N_AXIS] = { 0.0, 0.0, 0.0 };
// Custom $ settings
kinematic_segment_len = new FloatSetting(EXTENDED, WG, NULL, "Kinematics/SegmentLength", KINEMATIC_SEGMENT_LENGTH, 0.2, 5.0);
// Calculate the Z offset at the arm zero angles ...
// Z offset is the z distance from the motor axes to the end effector axes at zero angle
calc_forward_kinematics(angles, cartesian); // Sets the cartesian values
delta_z_offset = cartesian[Z_AXIS];
// print a startup message to show the kinematics are enabled. Print the offset for reference
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, delta_z_offset);
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]);
}
bool user_defined_homing() { // true = do not continue with normal Grbl homing
@@ -108,6 +115,7 @@ bool user_defined_homing() { // true = do not continue with normal Grbl homing
#endif
}
// This function is used by Grbl
void inverse_kinematics(float* position) {
float motor_angles[3];
delta_calcInverse(position, motor_angles);
@@ -116,6 +124,7 @@ void inverse_kinematics(float* position) {
position[2] = motor_angles[2];
}
// This function is used by Grbl
void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos
{
float dx, dy, dz; // distances in each cartesian axis
@@ -128,12 +137,12 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
KinematicError status;
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
//grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
//grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
status = delta_calcInverse(position, motor_angles);
if (status == KinematicError::OUT_OF_RANGE) {
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
//grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error %3.3f %3.3f %3.3f", position[0], position[1], position[2]);
start_position_error = true;
}
@@ -141,7 +150,6 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
status = delta_calcInverse(target, motor_angles);
if (status == KinematicError::OUT_OF_RANGE) {
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable error %3.3f %3.3f %3.3f", target[0], target[1], target[2]);
return;
}
position[X_AXIS] += gc_state.coord_offset[X_AXIS];
@@ -184,24 +192,40 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio
} else {
if (show_error) {
grbl_msg_sendf(CLIENT_SERIAL,
MsgLevel::Info,
"Error:%d, Angs X:%4.3f Y:%4.3f Z:%4.3f",
status,
motor_angles[0],
motor_angles[1],
motor_angles[2]);
// grbl_msg_sendf(CLIENT_SERIAL,
// MsgLevel::Info,
// "Error:%d, Angs X:%4.3f Y:%4.3f Z:%4.3f",
// status,
// motor_angles[0],
// motor_angles[1],
// motor_angles[2]);
show_error = false;
}
}
}
}
// this is used used by soft limits to see if the range of the machine is exceeded.
// this is used used by Grbl soft limits to see if the range of the machine is exceeded.
uint8_t kinematic_limits_check(float* target) {
float motor_angles[3];
return (delta_calcInverse(target, motor_angles) == KinematicError::NONE);
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin Soft Check %3.3f, %3.3f, %3.3f", target[0], target[1], target[2]);
KinematicError status = delta_calcInverse(target, motor_angles);
switch(status) {
case KinematicError::OUT_OF_RANGE:
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target out of range");
break;
case KinematicError::ANGLE_TOO_NEGATIVE:
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max negative");
break;
case KinematicError::ANGLE_TOO_POSITIVE:
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max positive");
break;
}
return (status == KinematicError::NONE);
}
// inverse kinematics: cartesian -> angles
@@ -299,6 +323,10 @@ KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta) {
return KinematicError::ANGLE_TOO_NEGATIVE;
}
if (theta > MAX_POSITIVE_ANGLE) {
return KinematicError::ANGLE_TOO_POSITIVE;
}
return KinematicError::NONE;
}
@@ -318,8 +346,8 @@ void forward_kinematics(float* position) {
memcpy(position_steps, sys_position, sizeof(sys_position));
system_convert_array_steps_to_mpos(position_radians, position_steps);
grbl_msg_sendf(
CLIENT_SERIAL, MsgLevel::Info, "Fwd Kin Angs %1.3f, %1.3f, %1.3f ", position_radians[0], position_radians[1], position_radians[2]);
// grbl_msg_sendf(
// CLIENT_SERIAL, MsgLevel::Info, "Fwd Kin Angs %1.3f, %1.3f, %1.3f ", position_radians[0], position_radians[1], position_radians[2]);
// detmine the position of the end effector joint center.
status = calc_forward_kinematics(position_radians, calc_fwd);

2
Grbl_Esp32/src/Config.h
View File

@@ -100,7 +100,7 @@ const int MAX_N_AXIS = 6;
#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

38
Grbl_Esp32/src/Machines/tapster_pro.h
View File

@@ -21,24 +21,6 @@
#define MACHINE_NAME "Tapster Pro Delta (Trinamic)"
#define CUSTOM_CODE_FILENAME "Custom/parallel_delta.cpp"
/*
// enable these special machine functions to be called from the main program
#define USE_KINEMATICS // there are kinematic equations for this machine
#define FWD_KINEMATICS_REPORTING // report in cartesian
#define USE_RMT_STEPS // Use the RMT periferal to generate step pulses
#define USE_TRINAMIC // some Trinamic motors are used on this machine
#define USE_MACHINE_TRINAMIC_INIT // there is a machine specific setup for the drivers
#define USE_MACHINE_INIT // There is some custom initialization for this machine
#define SEGMENT_LENGTH 0.5 // segment length in mm
#define KIN_ANGLE_CALC_OK 0
#define KIN_ANGLE_ERROR -1
#define MAX_NEGATIVE_ANGLE -36 // in degrees how far can the arms go up?
#define HOMING_CURRENT_REDUCTION 1.0
*/
#define N_AXIS 3
@@ -94,12 +76,20 @@
#define Y_LIMIT_PIN GPIO_NUM_39
#define Z_LIMIT_PIN GPIO_NUM_34
// Example (4x) 5V Buffer Output on socket #5
// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-5V-Buffered-Output-Module
#define COOLANT_MIST_PIN I2SO(24) // No PWM
#define COOLANT_FLOOD_PIN I2SO(25)
#define USER_DIGITAL_PIN_0 I2SO(26)
#define USER_DIGITAL_PIN_1 I2SO(27)
// Example Quad MOSFET module in socket #3
// https://github.com/bdring/6-Pack_CNC_Controller/wiki/Quad-MOSFET-Module
#define USER_DIGITAL_PIN_0 GPIO_NUM_26
#define USER_DIGITAL_PIN_1 GPIO_NUM_4
#define USER_DIGITAL_PIN_2 GPIO_NUM_16
#define USER_DIGITAL_PIN_3 GPIO_NUM_27
// Example Servo module in socket #4
// https://github.com/bdring/6-Pack_CNC_Controller/wiki/RC-Servo-BESC-CNC-I-O-Module
// https://github.com/bdring/Grbl_Esp32/wiki/M62,-M63,-M64,-M65-&-M67-User-I-O-Commands
#define USER_ANALOG_PIN_0 GPIO_NUM_14
#define USER_ANALOG_PIN_1 GPIO_NUM_13
#define USER_ANALOG_PIN_2 GPIO_NUM_15
#define USER_ANALOG_PIN_3 GPIO_NUM_12
// ================= defaults ===========================

1
Grbl_Esp32/src/ProcessSettings.cpp
View File

@@ -198,6 +198,7 @@ Error disable_alarm_lock(const char* value, WebUI::AuthenticationLevel auth_leve
if (system_check_safety_door_ajar()) {
return Error::CheckDoor;
}
grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "---Unlock---");
report_feedback_message(Message::AlarmUnlock);
sys.state = State::Idle;
// Don't run startup script. Prevents stored moves in startup from causing accidents.

3
Grbl_Esp32/src/Report.cpp
View File

@@ -339,7 +339,8 @@ void report_ngc_parameters(uint8_t client) {
if (report_inches->get()) {
tlo *= INCH_PER_MM;
}
ngc_rpt += String(tlo, 3);;
ngc_rpt += String(tlo, 3);
;
ngc_rpt += "]\r\n";
grbl_send(client, ngc_rpt.c_str());
report_probe_parameters(client);

4
Grbl_Esp32/src/System.cpp
View File

@@ -246,10 +246,6 @@ void system_convert_array_steps_to_mpos(float* position, int32_t* steps) {
uint8_t system_check_travel_limits(float* target) {
uint8_t idx;
#ifdef USE_KINEMATICS
return (!kinematic_limits_check(target)); // kinematic_limits_check(...) returns true if not exceeding limits
#endif
auto n_axis = number_axis->get();
for (idx = 0; idx < n_axis; idx++) {
float travel = axis_settings[idx]->max_travel->get();