bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

TPARA followup

Browse Source
This commit is contained in:
Scott Lahteine 2021-03-03 17:57:24 -06:00
parent a46e025725
commit dd388aedfd
6 changed files with 29 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Marlin/src/gcode/scara/M360-M364.cpp
View File

@ -31,7 +31,7 @@
inline bool SCARA_move_to_cal(const uint8_t delta_a, const uint8_t delta_b) {
if (IsRunning()) {
forward_kinematics_SCARA(delta_a, delta_b);
forward_kinematics(delta_a, delta_b);
do_blocking_move_to_xy(cartes);
return true;
}

2
Marlin/src/module/delta.cpp
View File

@ -177,7 +177,7 @@ float delta_safe_distance_from_top() {
*
* The result is stored in the cartes[] array.
*/
void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3) {
void forward_kinematics(const float &z1, const float &z2, const float &z3) {
// Create a vector in old coordinates along x axis of new coordinate
const float p12[3] = { delta_tower[B_AXIS].x - delta_tower[A_AXIS].x, delta_tower[B_AXIS].y - delta_tower[A_AXIS].y, z2 - z1 },

6
Marlin/src/module/delta.h
View File

@ -120,10 +120,10 @@ float delta_safe_distance_from_top();
*
* The result is stored in the cartes[] array.
*/
void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3);
void forward_kinematics(const float &z1, const float &z2, const float &z3);
FORCE_INLINE void forward_kinematics_DELTA(const abc_float_t &point) {
forward_kinematics_DELTA(point.a, point.b, point.c);
FORCE_INLINE void forward_kinematics(const abc_float_t &point) {
forward_kinematics(point.a, point.b, point.c);
}
void home_delta();

6
Marlin/src/module/motion.cpp
View File

@ -263,10 +263,10 @@ void sync_plan_position_e() { planner.set_e_position_mm(current_position.e); }
*/
void get_cartesian_from_steppers() {
#if ENABLED(DELTA)
forward_kinematics_DELTA(planner.get_axis_positions_mm());
forward_kinematics(planner.get_axis_positions_mm());
#else
#if IS_SCARA
forward_kinematics_SCARA(
forward_kinematics(
planner.get_axis_position_degrees(A_AXIS)
, planner.get_axis_position_degrees(B_AXIS)
#if ENABLED(AXEL_TPARA)
@ -949,7 +949,7 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
float x_home_pos(const uint8_t extruder) {
if (extruder == 0)
return base_home_pos(X_AXIS);
return X_HOME_POS;
else
/**
* In dual carriage mode the extruder offset provides an override of the

37
Marlin/src/module/scara.cpp
View File

@ -33,10 +33,7 @@
#include "planner.h"
#if ENABLED(AXEL_TPARA)
// For homing, as in delta
#include "planner.h"
#include "endstops.h"
#include "../lcd/marlinui.h"
#include "../MarlinCore.h"
#endif
@ -46,30 +43,35 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
if (axis == Z_AXIS)
current_position.z = Z_HOME_POS;
else {
xyz_pos_t homeposition;
LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
#if ENABLED(MORGAN_SCARA)
// MORGAN_SCARA uses arm angles for AB home position
ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
//DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
inverse_kinematics(homeposition);
forward_kinematics_SCARA(delta.a, delta.b);
current_position[axis] = cartes[axis];
#elif ENABLED(MP_SCARA)
// MP_SCARA uses a Cartesian XY home position
xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
//DEBUG_ECHOPGM("homeposition");
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y);
delta.a = SCARA_OFFSET_THETA1;
delta.b = SCARA_OFFSET_THETA2;
forward_kinematics_SCARA(delta.a, delta.b);
current_position[axis] = cartes[axis];
#elif ENABLED(AXEL_TPARA)
xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
//DEBUG_ECHOPGM("homeposition");
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z);
inverse_kinematics(homeposition);
forward_kinematics_TPARA(delta.a, delta.b, delta.c);
current_position[axis] = cartes[axis];
#endif
#if ENABLED(MORGAN_SCARA)
delta = homeposition;
#else
inverse_kinematics(homeposition);
#endif
#if EITHER(MORGAN_SCARA, MP_SCARA)
forward_kinematics(delta.a, delta.b);
#elif ENABLED(AXEL_TPARA)
forward_kinematics(delta.a, delta.b, delta.c);
#endif
current_position[axis] = cartes[axis];
//DEBUG_ECHOPGM("Cartesian");
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
update_software_endstops(axis);
@ -85,7 +87,7 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
* Maths and first version by QHARLEY.
* Integrated into Marlin and slightly restructured by Joachim Cerny.
*/
void forward_kinematics_SCARA(const float &a, const float &b) {
void forward_kinematics(const float &a, const float &b) {
const float a_sin = sin(RADIANS(a)) * L1,
a_cos = cos(RADIANS(a)) * L1,
b_sin = sin(RADIANS(b + TERN0(MP_SCARA, a))) * L2,
@ -174,7 +176,7 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
static constexpr xyz_pos_t robot_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z };
// Convert ABC inputs in degrees to XYZ outputs in mm
void forward_kinematics_TPARA(const float &a, const float &b, const float &c) {
void forward_kinematics(const float &a, const float &b, const float &c) {
const float w = c - b,
r = L1 * cos(RADIANS(b)) + L2 * sin(RADIANS(w - (90 - b))),
x = r * cos(RADIANS(a)),
@ -227,7 +229,6 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
homeaxis(C_AXIS);
homeaxis(B_AXIS);
// Set all carriages to their home positions
// Do this here all at once for Delta, because
// XYZ isn't ABC. Applying this per-tower would

4
Marlin/src/module/scara.h
View File

@ -35,7 +35,7 @@ extern float delta_segments_per_second;
L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2,
L2_2 = sq(float(L2));
void forward_kinematics_TPARA(const float &a, const float &b, const float &c);
void forward_kinematics(const float &a, const float &b, const float &c);
void home_TPARA();
#else
@ -44,7 +44,7 @@ extern float delta_segments_per_second;
L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2,
L2_2 = sq(float(L2));
void forward_kinematics_SCARA(const float &a, const float &b);
void forward_kinematics(const float &a, const float &b);
#endif