Skip to content

Commit

Permalink
Added kinematics homing
Browse files Browse the repository at this point in the history 
This allows the kinematics to do a special homing method, like wall plotter or delta.
  • Loading branch information
@bdring
bdring committed Aug 10, 2023
1 parent 051b1da commit 6af2505
Show file tree
Hide file tree
Showing 11 changed files with 59 additions and 43 deletions.
6 changes: 3 additions & 3 deletions FluidNC/src/Kinematics/Cartesian.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -305,9 +305,9 @@ namespace Kinematics {
}
}

// bool Cartesian::kinematics_homing(AxisMask& axisMask) {
// return false; // kinematics does not do the homing for catesian systems
// }
bool Cartesian::kinematics_homing(AxisMask& axisMask) {
return false; // kinematics does not do the homing for catesian systems
}

// Configuration registration
namespace {
Expand Down
2 changes: 1 addition & 1 deletion FluidNC/src/Kinematics/Cartesian.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ namespace Kinematics {
bool canHome(AxisMask axisMask) override;
void releaseMotors(AxisMask axisMask, MotorMask motors) override;
bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited) override;
//virtual bool kinematics_homing(AxisMask& axisMask) override;
virtual bool kinematics_homing(AxisMask& axisMask) override;

// Configuration handlers:
void afterParse() override {}
Expand Down
5 changes: 5 additions & 0 deletions FluidNC/src/Kinematics/Kinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,11 @@ namespace Kinematics {
return _system->canHome(axisMask);
}

bool Kinematics::kinematics_homing(AxisMask axisMask) {
Assert(_system != nullptr, "No kinematic system");
return _system->kinematics_homing(axisMask);
}

void Kinematics::releaseMotors(AxisMask axisMask, MotorMask motors) {
Assert(_system != nullptr, "No kinematic system");
_system->releaseMotors(axisMask, motors);
Expand Down
3 changes: 2 additions & 1 deletion FluidNC/src/Kinematics/Kinematics.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ namespace Kinematics {
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc);

bool canHome(AxisMask axisMask);
bool kinematics_homing(AxisMask axisMask);
void releaseMotors(AxisMask axisMask, MotorMask motors);
bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited);

Expand Down Expand Up @@ -89,7 +90,7 @@ namespace Kinematics {
virtual bool canHome(AxisMask axisMask) { return false; }
virtual void releaseMotors(AxisMask axisMask, MotorMask motors) {}
virtual bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited) { return false; }
//virtual bool kinematics_homing(AxisMask& axisMask) { return false; }
virtual bool kinematics_homing(AxisMask& axisMask) { return false; }

// Configuration interface.
void afterParse() override {}
Expand Down
32 changes: 16 additions & 16 deletions FluidNC/src/Kinematics/ParallelDelta.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -295,22 +295,22 @@ namespace Kinematics {
*/


// bool ParallelDelta::kinematics_homing(AxisMask& axisMask) {
// auto axes = config->_axes;
// auto n_axis = axes->_numberAxis;

// log_debug("kinematics_homing");
// config->_axes->set_disable(false);

// // TODO deal with non kinematic axes above Z
// for (int axis = 0; axis < 3; axis++) {
// //set_motor_steps(axis, mpos_to_steps(axes->_axis[axis]->_homing->_mpos, axis));
// int32_t steps = mpos_to_steps(_homing_mpos, axis);
// set_motor_steps(axis, steps);
// }
// protocol_disable_steppers();
// return true; // signal main code that this handled all homing
// }
bool ParallelDelta::kinematics_homing(AxisMask& axisMask) {
auto axes = config->_axes;
auto n_axis = axes->_numberAxis;

log_debug("kinematics_homing");
config->_axes->set_disable(false);

// TODO deal with non kinematic axes above Z
for (int axis = 0; axis < 3; axis++) {
//set_motor_steps(axis, mpos_to_steps(axes->_axis[axis]->_homing->_mpos, axis));
int32_t steps = mpos_to_steps(_homing_mpos, axis);
set_motor_steps(axis, steps);
}
protocol_disable_steppers();
return true; // signal main code that this handled all homing
}

// helper functions, calculates angle theta1 (for YZ-pane)
bool ParallelDelta::delta_calcAngleYZ(float x0, float y0, float z0, float& theta) {
Expand Down
2 changes: 1 addition & 1 deletion FluidNC/src/Kinematics/ParallelDelta.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ namespace Kinematics {
void motors_to_cartesian(float* cartesian, float* motors, int n_axis) override;
bool transform_cartesian_to_motors(float* motors, float* cartesian) override;
//bool soft_limit_error_exists(float* cartesian) override;
//bool kinematics_homing(AxisMask& axisMask) override;
bool kinematics_homing(AxisMask& axisMask) override;
virtual void constrain_jog(float* cartesian, plan_line_data_t* pl_data, float* position) override;
void releaseMotors(AxisMask axisMask, MotorMask motors) override;

Expand Down
6 changes: 3 additions & 3 deletions FluidNC/src/Kinematics/WallPlotter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -230,9 +230,9 @@ namespace Kinematics {
right_length = hypot_f(right_dx, right_dy);
}

// bool WallPlotter::kinematics_homing(AxisMask& axisMask) {
// return false; // kinematics does not do the homing for catesian systems
// }
bool WallPlotter::kinematics_homing(AxisMask& axisMask) {
return false; // kinematics does not do the homing for catesian systems
}

// Configuration registration
namespace {
Expand Down
2 changes: 1 addition & 1 deletion FluidNC/src/Kinematics/WallPlotter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ namespace Kinematics {
bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) override;
void motors_to_cartesian(float* cartesian, float* motors, int n_axis) override;
bool transform_cartesian_to_motors(float* cartesian, float* motors) override;
//bool kinematics_homing(AxisMask& axisMask) override;
bool kinematics_homing(AxisMask& axisMask) override;

// Configuration handlers:
void validate() override {}
Expand Down
7 changes: 4 additions & 3 deletions FluidNC/src/Machine/Homing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -432,9 +432,9 @@ namespace Machine {
// cycle. The protocol loop will then respond to events and advance
// the homing state machine through its phases.
void Homing::run_cycles(AxisMask axisMask) {
// if (config->_kinematics->kinematics_homing(axisMask)) {
// return;
// }
if (config->_kinematics->kinematics_homing(axisMask)) {
return;
}

if (!config->_kinematics->canHome(axisMask)) {
sys.state = State::Alarm;
Expand All @@ -445,6 +445,7 @@ namespace Machine {
auto n_axis = config->_axes->_numberAxis;
for (int axis = X_AXIS; axis < n_axis; axis++) {
if (config->_axes->_axis[axis]->_homing->_cycle == set_mpos_only) {
log_info("set_mpos_only");
if (axisMask == 0 || axisMask & 1 << axis) {
float* mpos = get_mpos();
mpos[axis] = config->_axes->_axis[axis]->_homing->_mpos;
Expand Down
15 changes: 10 additions & 5 deletions FluidNC/src/Motors/Dynamixel2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,16 +90,16 @@ namespace MotorDrivers {
if (len == PING_RSP_LEN) {
uint16_t model_num = _rx_message[10] << 8 | _rx_message[9];
uint8_t fw_rev = _rx_message[11];
std::string msg("Axis ping reply ");
std::string msg(" ");
msg += axisName().c_str();
if (model_num == 1060) {
msg += " Model XL430-W250";
msg += "reply: Model XL430-W250";
} else {
msg += " M/N " + std::to_string(model_num);
}
log_info(msg << " F/W Rev " << to_hex(fw_rev));
} else {
log_warn(" Ping failed");
log_warn(axisName() << " Ping failed");
return false;
}

Expand All @@ -110,6 +110,7 @@ namespace MotorDrivers {

// sets the PWM to zero. This allows most servos to be manually moved
void IRAM_ATTR Dynamixel2::set_disable(bool disable) {
log_info("dyn disable:"<<disable)
if (_disabled == disable) {
return;
}
Expand Down Expand Up @@ -158,7 +159,9 @@ namespace MotorDrivers {
}
finish_message();
}
void Dynamixel2::update() { update_all(); }
void Dynamixel2::update() {
update_all();
}

void Dynamixel2::set_location() {}

Expand All @@ -177,7 +180,9 @@ namespace MotorDrivers {
return false; // Cannot do conventional homing
}

void Dynamixel2::add_uint8(uint8_t n) { _tx_message[_msg_index++] = n & 0xff; }
void Dynamixel2::add_uint8(uint8_t n) {
_tx_message[_msg_index++] = n & 0xff;
}
void Dynamixel2::add_uint16(uint16_t n) {
add_uint8(n);
add_uint8(n >> 8);
Expand Down
22 changes: 13 additions & 9 deletions example_configs/TapDelta_XYZ.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,13 @@ start:
deactivate_parking: false
check_limits: false

uart1:
txd_pin: gpio.4
rxd_pin: gpio.13
rts_pin: gpio.17
baud: 1000000
mode: 8N1

axes:
shared_stepper_disable_pin: NO_PIN
x:
Expand All @@ -41,19 +48,14 @@ axes:
max_travel_mm: 2.094
soft_limits: false
homing:
cycle: 1
cycle: -1
positive_direction: false
mpos_mm: -0.524

motor0:
dynamixel2:
uart_num: 1
id: 1
uart:
txd_pin: gpio.4
rxd_pin: gpio.13
rts_pin: gpio.17
baud: 1000000
mode: 8N1
count_min: 1707
count_max: 3072
timer_ms: 30
Expand All @@ -65,12 +67,13 @@ axes:
max_travel_mm: 2.094
soft_limits: false
homing:
cycle: 1
cycle: -1
positive_direction: false
mpos_mm: -0.524

motor0:
dynamixel2:
uart_num: 1
id: 2
count_min: 1707
count_max: 3072
Expand All @@ -82,12 +85,13 @@ axes:
max_travel_mm: 2.094
soft_limits: false
homing:
cycle: 1
cycle: -1
positive_direction: false
mpos_mm: -0.524

motor0:
dynamixel2:
uart_num: 1
id: 3
count_min: 1707
count_max: 3072
Expand Down

0 comments on commit 6af2505

Please sign in to comment.