andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Patch to fix kinematics

pull/1/head
Scott Lahteine 8 years ago
parent
e529c6407e
commit
d65f5d816f
2 changed files with 73 additions and 58 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 24
      Marlin/Marlin.h
  2. 107
      Marlin/Marlin_main.cpp

24
Marlin/Marlin.h
View File

@ -302,22 +302,24 @@ int code_value_int();
float code_value_temp_abs();
float code_value_temp_diff();
#if ENABLED(DELTA)
#if IS_KINEMATIC
extern float delta[ABC];
extern float endstop_adj[ABC]; // axis[n].endstop_adj
extern float delta_radius;
extern float delta_diagonal_rod;
extern float delta_segments_per_second;
extern float delta_diagonal_rod_trim_tower_1;
extern float delta_diagonal_rod_trim_tower_2;
extern float delta_diagonal_rod_trim_tower_3;
void inverse_kinematics(const float cartesian[XYZ]);
#endif
#if ENABLED(DELTA)
extern float delta[ABC],
endstop_adj[ABC],
delta_radius,
delta_diagonal_rod,
delta_segments_per_second,
delta_diagonal_rod_trim_tower_1,
delta_diagonal_rod_trim_tower_2,
delta_diagonal_rod_trim_tower_3;
void recalc_delta_settings(float radius, float diagonal_rod);
#elif IS_SCARA
extern float delta[ABC];
extern float axis_scaling[ABC]; // Build size scaling
void inverse_kinematics(const float cartesian[XYZ]);
void forward_kinematics_SCARA(float f_scara[ABC]);
void forward_kinematics_SCARA(const float &a, const float &b);
#endif
#if ENABLED(AUTO_BED_LEVELING_NONLINEAR)

107
Marlin/Marlin_main.cpp
View File

@ -465,7 +465,6 @@ static uint8_t target_extruder;
#define COS_60 0.5
float delta[ABC],
cartes[XYZ] = { 0 },
endstop_adj[ABC] = { 0 };
// these are the default values, can be overriden with M665
@ -487,7 +486,6 @@ static uint8_t target_extruder;
delta_clip_start_height = Z_MAX_POS;
float delta_safe_distance_from_top();
void get_cartesian_from_steppers();
#else
@ -508,11 +506,11 @@ static uint8_t target_extruder;
float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND,
delta[ABC],
axis_scaling[ABC] = { 1, 1, 1 }, // Build size scaling, default to 1
cartes[XYZ] = { 0 };
void get_cartesian_from_steppers() { } // to be written later
axis_scaling[ABC] = { 1, 1, 1 }; // Build size scaling, default to 1
#endif
float cartes[XYZ] = { 0 };
#if ENABLED(FILAMENT_WIDTH_SENSOR)
bool filament_sensor = false; //M405 turns on filament_sensor control, M406 turns it off
float filament_width_nominal = DEFAULT_NOMINAL_FILAMENT_DIA, // Nominal filament width. Change with M404
@ -598,6 +596,8 @@ void stop();
void get_available_commands();
void process_next_command();
void prepare_move_to_destination();
void get_cartesian_from_steppers();
void set_current_from_steppers_for_axis(AxisEnum axis);
#if ENABLED(ARC_SUPPORT)
@ -1347,7 +1347,7 @@ static void set_axis_is_at_home(AxisEnum axis) {
}
#endif
#if ENABLED(SCARA)
#if ENABLED(MORGAN_SCARA)
if (axis == X_AXIS || axis == Y_AXIS) {
@ -1362,19 +1362,19 @@ static void set_axis_is_at_home(AxisEnum axis) {
* and calculates homing offset using forward kinematics
*/
inverse_kinematics(homeposition);
forward_kinematics_SCARA(delta);
forward_kinematics_SCARA(delta[A_AXIS], delta[B_AXIS]);
// SERIAL_ECHOPAIR("Delta X=", delta[X_AXIS]);
// SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]);
// SERIAL_ECHOPAIR("Delta X=", cartes[X_AXIS]);
// SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(cartes[Y_AXIS]);
current_position[axis] = LOGICAL_POSITION(delta[axis], axis);
current_position[axis] = LOGICAL_POSITION(cartes[axis], axis);
/**
* SCARA home positions are based on configuration since the actual
* limits are determined by the inverse kinematic transform.
*/
soft_endstop_min[axis] = base_min_pos(axis); // + (delta[axis] - base_home_pos(axis));
soft_endstop_max[axis] = base_max_pos(axis); // + (delta[axis] - base_home_pos(axis));
soft_endstop_min[axis] = base_min_pos(axis); // + (cartes[axis] - base_home_pos(axis));
soft_endstop_max[axis] = base_max_pos(axis); // + (cartes[axis] - base_home_pos(axis));
}
else
#endif
@ -5089,7 +5089,7 @@ static void report_current_position() {
stepper.report_positions();
#if ENABLED(SCARA)
#if IS_SCARA
SERIAL_PROTOCOLPGM("SCARA Theta:");
SERIAL_PROTOCOL(delta[X_AXIS]);
SERIAL_PROTOCOLPGM(" Psi+Theta:");
@ -5327,7 +5327,7 @@ inline void gcode_M206() {
if (code_seen(axis_codes[i]))
set_home_offset((AxisEnum)i, code_value_axis_units(i));
#if ENABLED(SCARA)
#if IS_SCARA
if (code_seen('T')) set_home_offset(X_AXIS, code_value_axis_units(X_AXIS)); // Theta
if (code_seen('P')) set_home_offset(Y_AXIS, code_value_axis_units(Y_AXIS)); // Psi
#endif
@ -5808,17 +5808,16 @@ inline void gcode_M303() {
#endif
}
#if ENABLED(SCARA)
bool SCARA_move_to_cal(uint8_t delta_x, uint8_t delta_y) {
#if ENABLED(MORGAN_SCARA)
bool SCARA_move_to_cal(uint8_t delta_a, uint8_t delta_b) {
//SoftEndsEnabled = false; // Ignore soft endstops during calibration
//SERIAL_ECHOLNPGM(" Soft endstops disabled");
if (IsRunning()) {
//gcode_get_destination(); // For X Y Z E F
delta[X_AXIS] = delta_x;
delta[Y_AXIS] = delta_y;
forward_kinematics_SCARA(delta);
destination[X_AXIS] = delta[X_AXIS] / axis_scaling[X_AXIS];
destination[Y_AXIS] = delta[Y_AXIS] / axis_scaling[Y_AXIS];
forward_kinematics_SCARA(delta_a, delta_b);
destination[X_AXIS] = cartes[X_AXIS] / axis_scaling[X_AXIS];
destination[Y_AXIS] = cartes[Y_AXIS] / axis_scaling[Y_AXIS];
destination[Z_AXIS] = current_position[Z_AXIS];
prepare_move_to_destination();
//ok_to_send();
return true;
@ -7456,7 +7455,7 @@ void process_next_command() {
gcode_M303();
break;
#if ENABLED(SCARA)
#if ENABLED(MORGAN_SCARA)
case 360: // M360 SCARA Theta pos1
if (gcode_M360()) return;
break;
@ -7794,12 +7793,6 @@ void ok_to_send() {
forward_kinematics_DELTA(point[A_AXIS], point[B_AXIS], point[C_AXIS]);
}
void get_cartesian_from_steppers() {
forward_kinematics_DELTA(stepper.get_axis_position_mm(A_AXIS),
stepper.get_axis_position_mm(B_AXIS),
stepper.get_axis_position_mm(C_AXIS));
}
#if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
// Adjust print surface height by linear interpolation over the bed_level array.
@ -8274,32 +8267,32 @@ void prepare_move_to_destination() {
#endif // HAS_CONTROLLERFAN
#if ENABLED(SCARA)
#if IS_SCARA
void forward_kinematics_SCARA(float f_scara[ABC]) {
// Perform forward kinematics, and place results in delta[]
void forward_kinematics_SCARA(const float &a, const float &b) {
// Perform forward kinematics, and place results in cartes[]
// The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
float x_sin, x_cos, y_sin, y_cos;
float a_sin, a_cos, b_sin, b_cos;
//SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(f_scara[X_AXIS]);
//SERIAL_ECHOPGM(" y="); SERIAL_ECHO(f_scara[Y_AXIS]);
//SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(a);
//SERIAL_ECHOPGM(" y="); SERIAL_ECHO(b);
x_sin = sin(RADIANS(f_scara[X_AXIS])) * L1;
x_cos = cos(RADIANS(f_scara[X_AXIS])) * L1;
y_sin = sin(RADIANS(f_scara[Y_AXIS])) * L2;
y_cos = cos(RADIANS(f_scara[Y_AXIS])) * L2;
a_sin = sin(RADIANS(a)) * L1;
a_cos = cos(RADIANS(a)) * L1;
b_sin = sin(RADIANS(b)) * L2;
b_cos = cos(RADIANS(b)) * L2;
//SERIAL_ECHOPGM(" x_sin="); SERIAL_ECHO(x_sin);
//SERIAL_ECHOPGM(" x_cos="); SERIAL_ECHO(x_cos);
//SERIAL_ECHOPGM(" y_sin="); SERIAL_ECHO(y_sin);
//SERIAL_ECHOPGM(" y_cos="); SERIAL_ECHOLN(y_cos);
//SERIAL_ECHOPGM(" a_sin="); SERIAL_ECHO(a_sin);
//SERIAL_ECHOPGM(" a_cos="); SERIAL_ECHO(a_cos);
//SERIAL_ECHOPGM(" b_sin="); SERIAL_ECHO(b_sin);
//SERIAL_ECHOPGM(" b_cos="); SERIAL_ECHOLN(b_cos);
delta[X_AXIS] = x_cos + y_cos + SCARA_OFFSET_X; //theta
delta[Y_AXIS] = x_sin + y_sin + SCARA_OFFSET_Y; //theta+phi
cartes[X_AXIS] = a_cos + b_cos + SCARA_OFFSET_X; //theta
cartes[Y_AXIS] = a_sin + b_sin + SCARA_OFFSET_Y; //theta+phi
//SERIAL_ECHOPGM(" delta[X_AXIS]="); SERIAL_ECHO(delta[X_AXIS]);
//SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
//SERIAL_ECHOPGM(" cartes[X_AXIS]="); SERIAL_ECHO(cartes[X_AXIS]);
//SERIAL_ECHOPGM(" cartes[Y_AXIS]="); SERIAL_ECHOLN(cartes[Y_AXIS]);
}
void inverse_kinematics(const float cartesian[XYZ]) {
@ -8343,7 +8336,27 @@ void prepare_move_to_destination() {
//*/
}
#endif // MORGAN_SCARA
#endif // IS_SCARA
void get_cartesian_from_steppers() {
#if ENABLED(DELTA)
forward_kinematics_DELTA(
stepper.get_axis_position_mm(A_AXIS),
stepper.get_axis_position_mm(B_AXIS),
stepper.get_axis_position_mm(C_AXIS)
);
#elif IS_SCARA
forward_kinematics_SCARA(
stepper.get_axis_position_degrees(A_AXIS),
stepper.get_axis_position_degrees(B_AXIS)
);
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
#else
cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
#endif
}
#if ENABLED(TEMP_STAT_LEDS)

Write Preview
Loading…
Cancel
Save