TPARA followup

This commit is contained in:
Scott Lahteine 2021-03-03 17:57:24 -06:00 committed by Scott Lahteine
parent 864d27d460
commit d61e7dd685
6 changed files with 29 additions and 28 deletions

View File

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

View File

@ -177,7 +177,7 @@ float delta_safe_distance_from_top() {
* *
* The result is stored in the cartes[] array. * 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 // 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 }, 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 },

View File

@ -120,10 +120,10 @@ float delta_safe_distance_from_top();
* *
* The result is stored in the cartes[] array. * 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) { FORCE_INLINE void forward_kinematics(const abc_float_t &point) {
forward_kinematics_DELTA(point.a, point.b, point.c); forward_kinematics(point.a, point.b, point.c);
} }
void home_delta(); void home_delta();

View File

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

View File

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

View File

@ -35,7 +35,7 @@ extern float delta_segments_per_second;
L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2, L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2,
L2_2 = sq(float(L2)); 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(); void home_TPARA();
#else #else
@ -44,7 +44,7 @@ extern float delta_segments_per_second;
L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2, L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2,
L2_2 = sq(float(L2)); 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 #endif