Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-09-23 21:38:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Automatic multi-move homing when motors share an axis

Browse Source 
IMPORTANT: Requires that moves of all motors of an axis will trigger the same input!

IOW: If motors 2 and 3 are both on the Y axis, you will get three moves:

1. One with BOTH motors until it triggers the input
2. One with motor 2 while motor 3 is "frozen" until the input is triggered
3. One last move with motor 3 while motor 2 is "frozen" until the input is triggered

In order for this to work with two switches, you'll need the input set to `IO_ACTIVE_HIGH` with a physical pull DOWN (to ground), and *both( switches wired with the common side to the input, and the Normally Open (N.O.) side to logic `HIGH` (3.3v or 5v, depending on the board and jumpers, etc.). This way  when either or both switch is tripped it'll set the input to `HIGH`.
This commit is contained in:
Rob Giseburt
2023-02-17 03:13:21 +00:00
parent b91094bb4f
commit 06b0309e47
3 changed files with 165 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/cycle_homing.cpp
+107 -82
View File
@@ -50,6 +50,8 @@ struct hmHomingSingleton { // persistent homing runtime variables
stat_t (*func)(int8_t axis); // binding for callback function state machine
bool axis_flags[AXES]; // local storage for axis flags
uint8_t axis_moves_needed;
uint8_t axis_move;
// per-axis parameters
float direction; // set to 1 for positive (max), -1 for negative (to min);
@@ -199,6 +201,8 @@ stat_t cm_homing_cycle_start(const float axes[], const bool flags[]) {
hm.axis = -1; // set to retrieve initial axis
hm.func = _homing_axis_start; // bind initial processing function
hm.axis_moves_needed = 0;
hm.axis_move = 0;
cm->machine_state = MACHINE_CYCLE;
cm->cycle_type = CYCLE_HOMING;
cm->homing_state = HOMING_NOT_HOMED;
@@ -303,7 +307,7 @@ static stat_t _homing_axis_start(int8_t axis) {
hm.homing_input = cm->a[axis].homing_input;
din_handlers[INPUT_ACTION_INTERNAL].registerHandler(&_homing_handler);
hm.axis = axis; // persist the axis
hm.axis = axis; // persist the axis
hm.search_velocity = std::abs(cm->a[axis].search_velocity); // search velocity is always positive
hm.latch_velocity = std::abs(cm->a[axis].latch_velocity); // latch velocity is always positive
@@ -311,18 +315,18 @@ static stat_t _homing_axis_start(int8_t axis) {
// setup parameters for positive or negative travel (homing to the max or min switch)
if (homing_to_max) {
hm.search_travel = travel_distance; // search travels in positive direction
hm.latch_backoff = std::abs(cm->a[axis].latch_backoff); // latch travels in positive direction
hm.zero_backoff = -std::max(0.0f, cm->a[axis].zero_backoff);// zero backoff is negative direction (or zero)
// will set the maximum position
// (plus any negative backoff)
hm.search_travel = travel_distance; // search travels in positive direction
hm.latch_backoff = std::abs(cm->a[axis].latch_backoff); // latch travels in positive direction
hm.zero_backoff = -std::max(0.0f, cm->a[axis].zero_backoff); // zero backoff is negative direction (or zero)
// will set the maximum position
// (plus any negative backoff)
hm.setpoint = cm->a[axis].travel_max + (std::max(0.0f, -cm->a[axis].zero_backoff));
} else {
hm.search_travel = -travel_distance; // search travels in negative direction
hm.latch_backoff = -std::abs(cm->a[axis].latch_backoff); // latch travels in negative direction
hm.search_travel = -travel_distance; // search travels in negative direction
hm.latch_backoff = -std::abs(cm->a[axis].latch_backoff); // latch travels in negative direction
hm.zero_backoff = std::max(0.0f, cm->a[axis].zero_backoff); // zero backoff is positive direction (or zero)
// will set the minimum position
// (minus any negative backoff)
// will set the minimum position
// (minus any negative backoff)
hm.setpoint = cm->a[axis].travel_min + (std::max(0.0f, -cm->a[axis].zero_backoff));
}
@@ -492,142 +496,163 @@ static stat_t _homing_finalize_exit(int8_t axis) // third part of return to hom
*/
static int8_t _get_next_axis(int8_t axis) {
if (hm.axis_moves_needed > hm.axis_move) {
kn->axis_set_homing_move(axis, hm.axis_move);
hm.axis_move++;
return axis;
}
auto ret_axis = -1; // done
#if (HOMING_AXES <= 4)
if (axis == -1) { // inelegant brute force solution
if (hm.axis_flags[AXIS_Z]) {
return (AXIS_Z);
}
ret_axis = AXIS_Z;
} else
if (hm.axis_flags[AXIS_X]) {
return (AXIS_X);
}
ret_axis = AXIS_X;
} else
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
ret_axis = AXIS_Y;
} else
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
ret_axis = AXIS_A;
} else
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
ret_axis = AXIS_B;
} else
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
ret_axis = AXIS_C;
} else {
ret_axis = -2; // error
}
return (-2); // error
} else if (axis == AXIS_Z) {
if (hm.axis_flags[AXIS_X]) {
return (AXIS_X);
}
ret_axis = AXIS_X;
} else
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
ret_axis = AXIS_Y;
} else
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
ret_axis = AXIS_A;
} else
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
ret_axis = AXIS_B;
} else
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
ret_axis = AXIS_C;
}
} else if (axis == AXIS_X) {
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
ret_axis = AXIS_Y;
} else
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
ret_axis = AXIS_A;
} else
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
ret_axis = AXIS_B;
} else
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
ret_axis = AXIS_C;
}
} else if (axis == AXIS_Y) {
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
ret_axis = AXIS_A;
} else
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
ret_axis = AXIS_B;
} else
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
ret_axis = AXIS_C;
}
}
return (-1); // done
#else
if (axis == -1) {
if (hm.axis_flags[AXIS_Z]) {
return (AXIS_Z);
}
ret_axis = AXIS_Z;
} else
if (hm.axis_flags[AXIS_X]) {
return (AXIS_X);
}
ret_axis = AXIS_X;
} else
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
ret_axis = AXIS_Y;
} else
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
ret_axis = AXIS_A;
} else
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
ret_axis = AXIS_B;
} else
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
ret_axis = AXIS_C;
}
return (-2); // error
// ret_axis = -2; // error
} else if (axis == AXIS_Z) {
if (hm.axis_flags[AXIS_X]) {
return (AXIS_X);
}
ret_axis = AXIS_X;
} else
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
ret_axis = AXIS_Y;
} else
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
ret_axis = AXIS_A;
} else
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
ret_axis = AXIS_B;
} else
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
ret_axis = AXIS_C;
}
} else if (axis == AXIS_X) {
if (hm.axis_flags[AXIS_Y]) {
return (AXIS_Y);
}
ret_axis = AXIS_Y;
} else
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
ret_axis = AXIS_A;
} else
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
ret_axis = AXIS_B;
} else
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
ret_axis = AXIS_C;
}
} else if (axis == AXIS_Y) {
if (hm.axis_flags[AXIS_A]) {
return (AXIS_A);
}
ret_axis = AXIS_A;
} else
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
ret_axis = AXIS_B;
} else
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
ret_axis = AXIS_C;
}
} else if (axis == AXIS_A) {
if (hm.axis_flags[AXIS_B]) {
return (AXIS_B);
}
ret_axis = AXIS_B;
} else
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
ret_axis = AXIS_C;
}
} else if (axis == AXIS_B) {
if (hm.axis_flags[AXIS_C]) {
return (AXIS_C);
ret_axis = AXIS_C;
}
} else {
ret_axis = -1; // done
}
return (-1); // done
#endif // (HOMING_AXES <= 4)
if (ret_axis > 0) {
hm.axis_moves_needed = kn->axis_homing_moves(ret_axis);
hm.axis_move = 0;
kn->axis_set_homing_move(ret_axis, hm.axis_move);
hm.axis_move++;
} else {
hm.axis_moves_needed = 0;
hm.axis_move = 0;
kn->set_homing_done();
}
return ret_axis;
}
g2core/kinematics.h
+12
View File
@@ -75,6 +75,18 @@ struct KinematicsBase {
// sync any external sensors with the current step position
virtual void sync_encoders(const float step_position[motors], const float position[axes]);
// homing support - since kinematics is between the stepper and the gcode, we put this here
// how many moves will this axis need to home? normal is 1, but if it has two motors on it it may need 3 (see cartesian override)
virtual uint8_t axis_homing_moves(uint8_t axis) { return 1; }
// set which of the axis moves will be homed next - may freeze motors or change the kinematics output somehow
// if move > axis_homing_moves(axis) then it clears homing for that axis
virtual void axis_set_homing_move(uint8_t axis, uint8_t move) { /* nothing to do */ }
// called to give the opportunity to clean up after any homing setups
virtual void set_homing_done() { /* nothing to do here */ }
};
extern KinematicsBase<AXES, MOTORS> *kn;
g2core/kinematics_cartesian.h
+46 -4
View File
@@ -62,13 +62,15 @@ struct CartesianKinematics : KinematicsBase<axes, motors> {
float motor_offset[motors];
bool needs_sync_encoders = true; // if true, we need to update the steps_offset
int8_t motor_map[motors]; // for each motor, which joint it maps from
bool hold_motor[motors]; // true if the motor is to be held when the axis moves - for homing only ATM
float joint_position[joints];
float last_known_steps[motors];
void configure(const float new_steps_per_unit[motors], const int8_t new_motor_map[motors]) override
{
for (uint8_t motor = 0; motor < motors; motor++) {
motor_map[motor] = new_motor_map[motor];
hold_motor[motor] = false;
int8_t joint = motor_map[motor];
if (joint == -1) {
motor_offset[motor] = 0;
@@ -91,7 +93,13 @@ struct CartesianKinematics : KinematicsBase<axes, motors> {
continue;
}
steps[motor] = (target[joint] * steps_per_unit[motor]) + motor_offset[motor];
if (hold_motor[motor]) {
steps[motor] = last_known_steps[motor];
motor_offset[motor] = steps[motor] - (target[joint] * steps_per_unit[motor]);
} else {
steps[motor] = (target[joint] * steps_per_unit[motor]) + motor_offset[motor];
last_known_steps[motor] = steps[motor];
}
}
for (uint8_t joint = 0; joint < joints; joint++) {
@@ -148,7 +156,41 @@ struct CartesianKinematics : KinematicsBase<axes, motors> {
motor_offset[motor] = step_position[motor] - (position[joint] * steps_per_unit[motor]);
}
}
};
virtual uint8_t axis_homing_moves(uint8_t axis) {
uint8_t motors_used = 0;
for (uint8_t motor = 0; motor < motors; motor++) {
int8_t joint = motor_map[motor];
if (joint == axis) { motors_used++; }
}
// if we have two or more motors, we need an additional (first) move for all motors together
if (motors_used > 1) {
motors_used++;
}
return motors_used;
}
void axis_set_homing_move(uint8_t axis, uint8_t move) override {
uint8_t motors_seen = 0;
for (uint8_t motor = 0; motor < motors; motor++) {
int8_t joint = motor_map[motor];
// move == 0 means move all motors
// move == 1 mean hold all but the first motor
// move == 2 mean hold all but the second motor
if (joint == axis) {
motors_seen++;
hold_motor[motor] = (move != 0) && (motors_seen != move);
}
}
}
void set_homing_done() override {
for (uint8_t motor = 0; motor < motors; motor++) {
hold_motor[motor] = false;
}
}
}; // CartesianKinematics
// Support for CoreXY Kinematics - http://corexy.com/
@@ -212,6 +254,6 @@ struct CoreXYKinematics final : CartesianKinematics<axes, motors> {
position[0] = 0.5 * (deltaA + deltaB);
position[1] = 0.5 * (deltaA - deltaB);
}
};
}; // CoreXYKinematics
#endif // End of include Guard: KINEMATICS_CARTESIAN_H_ONCE