#include "ExtruderPublicAccess.h"
#include "GcodeDispatch.h"
#include "ActuatorCoordinates.h"
+#include "EndstopsPublicAccess.h"
#include "mbed.h" // for us_ticker_read()
#include "mri.h"
#define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
#define segment_z_moves_checksum CHECKSUM("segment_z_moves")
#define save_g92_checksum CHECKSUM("save_g92")
+#define save_g54_checksum CHECKSUM("save_g54")
#define set_g92_checksum CHECKSUM("set_g92")
// arm solutions
#define dir_pin_checksum CHEKCSUM("dir_pin")
#define en_pin_checksum CHECKSUM("en_pin")
-#define steps_per_mm_checksum CHECKSUM("steps_per_mm")
-#define max_rate_checksum CHECKSUM("max_rate")
+#define max_speed_checksum CHECKSUM("max_speed")
#define acceleration_checksum CHECKSUM("acceleration")
#define z_acceleration_checksum CHECKSUM("z_acceleration")
#define laser_module_default_power_checksum CHECKSUM("laser_module_default_power")
-#define ARC_ANGULAR_TRAVEL_EPSILON 5E-9F // Float (radians)
+#define enable_checksum CHECKSUM("enable")
+#define halt_checksum CHECKSUM("halt")
+#define soft_endstop_checksum CHECKSUM("soft_endstop")
+#define xmin_checksum CHECKSUM("x_min")
+#define ymin_checksum CHECKSUM("y_min")
+#define zmin_checksum CHECKSUM("z_min")
+#define xmax_checksum CHECKSUM("x_max")
+#define ymax_checksum CHECKSUM("y_max")
+#define zmax_checksum CHECKSUM("z_max")
+
#define PI 3.14159265358979323846F // force to be float, do not use M_PI
// The Robot converts GCodes into actual movements, and then adds them to the Planner, which passes them to the Conveyor so they can be added to the queue
this->max_speeds[X_AXIS] = THEKERNEL->config->value(x_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
this->max_speeds[Y_AXIS] = THEKERNEL->config->value(y_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
this->max_speeds[Z_AXIS] = THEKERNEL->config->value(z_axis_max_speed_checksum )->by_default( 300.0F)->as_number() / 60.0F;
+ this->max_speed = THEKERNEL->config->value(max_speed_checksum )->by_default( -60.0F)->as_number() / 60.0F;
this->segment_z_moves = THEKERNEL->config->value(segment_z_moves_checksum )->by_default(true)->as_bool();
this->save_g92 = THEKERNEL->config->value(save_g92_checksum )->by_default(false)->as_bool();
+ this->save_g54 = THEKERNEL->config->value(save_g54_checksum )->by_default(THEKERNEL->is_grbl_mode())->as_bool();
string g92 = THEKERNEL->config->value(set_g92_checksum )->by_default("")->as_string();
if(!g92.empty()) {
// optional setting for a fixed G92 offset
this->s_value = THEKERNEL->config->value(laser_module_default_power_checksum)->by_default(0.8F)->as_number();
// Make our Primary XYZ StepperMotors, and potentially A B C
- uint16_t const checksums[][6] = {
+ uint16_t const motor_checksums[][6] = {
ACTUATOR_CHECKSUMS("alpha"), // X
ACTUATOR_CHECKSUMS("beta"), // Y
ACTUATOR_CHECKSUMS("gamma"), // Z
for (size_t a = 0; a < MAX_ROBOT_ACTUATORS; a++) {
Pin pins[3]; //step, dir, enable
for (size_t i = 0; i < 3; i++) {
- pins[i].from_string(THEKERNEL->config->value(checksums[a][i])->by_default("nc")->as_string())->as_output();
+ pins[i].from_string(THEKERNEL->config->value(motor_checksums[a][i])->by_default("nc")->as_string())->as_output();
}
if(!pins[0].connected() || !pins[1].connected()) { // step and dir must be defined, but enable is optional
return;
}
- actuators[a]->change_steps_per_mm(THEKERNEL->config->value(checksums[a][3])->by_default(a == 2 ? 2560.0F : 80.0F)->as_number());
- actuators[a]->set_max_rate(THEKERNEL->config->value(checksums[a][4])->by_default(30000.0F)->as_number()/60.0F); // it is in mm/min and converted to mm/sec
- actuators[a]->set_acceleration(THEKERNEL->config->value(checksums[a][5])->by_default(NAN)->as_number()); // mm/secs²
+ actuators[a]->change_steps_per_mm(THEKERNEL->config->value(motor_checksums[a][3])->by_default(a == 2 ? 2560.0F : 80.0F)->as_number());
+ actuators[a]->set_max_rate(THEKERNEL->config->value(motor_checksums[a][4])->by_default(30000.0F)->as_number()/60.0F); // it is in mm/min and converted to mm/sec
+ actuators[a]->set_acceleration(THEKERNEL->config->value(motor_checksums[a][5])->by_default(NAN)->as_number()); // mm/secs²
}
check_max_actuator_speeds(); // check the configs are sane
// so the first move can be correct if homing is not performed
ActuatorCoordinates actuator_pos;
arm_solution->cartesian_to_actuator(machine_position, actuator_pos);
- for (size_t i = 0; i < n_motors; i++)
+ for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
actuators[i]->change_last_milestone(actuator_pos[i]);
+ }
+
+ // initialize any extra axis to machine position
+ for (size_t i = A_AXIS; i < n_motors; i++) {
+ actuators[i]->change_last_milestone(machine_position[i]);
+ }
//this->clearToolOffset();
+
+ soft_endstop_enabled= THEKERNEL->config->value(soft_endstop_checksum, enable_checksum)->by_default(false)->as_bool();
+ soft_endstop_halt= THEKERNEL->config->value(soft_endstop_checksum, halt_checksum)->by_default(true)->as_bool();
+
+ soft_endstop_min[X_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, xmin_checksum)->by_default(NAN)->as_number();
+ soft_endstop_min[Y_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, ymin_checksum)->by_default(NAN)->as_number();
+ soft_endstop_min[Z_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, zmin_checksum)->by_default(NAN)->as_number();
+ soft_endstop_max[X_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, xmax_checksum)->by_default(NAN)->as_number();
+ soft_endstop_max[Y_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, ymax_checksum)->by_default(NAN)->as_number();
+ soft_endstop_max[Z_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, zmax_checksum)->by_default(NAN)->as_number();
}
uint8_t Robot::register_motor(StepperMotor *motor)
actuators[selected_extruder]->change_last_milestone(get_e_scale_fnc ? e*get_e_scale_fnc() : e);
}
}
+ if(gcode->subcode == 0 && gcode->get_num_args() > 0) {
+ for (int i = A_AXIS; i < n_motors; i++) {
+ // ABC just need to set machine_position and compensated_machine_position if specified
+ char axis= 'A'+i-3;
+ if(!actuators[i]->is_extruder() && gcode->has_letter(axis)) {
+ float ap= gcode->get_value(axis);
+ machine_position[i]= compensated_machine_position[i]= ap;
+ actuators[i]->change_last_milestone(ap); // this updates the last_milestone in the actuator
+ }
+ }
+ }
#endif
return;
if(actuators[i]->is_extruder()) continue; //extruders handle this themselves
gcode->stream->printf(" %c: %g ", 'A' + i - A_AXIS, actuators[i]->get_max_rate());
}
+ }else{
+ gcode->stream->printf(" S: %g ", this->max_speed);
}
gcode->add_nl = true;
actuators[i]->set_max_rate(v);
}
}
+
+ }else{
+ if(gcode->has_letter('S')) max_speed= gcode->get_value('S');
}
}
break;
+ case 211: // M211 Sn turns soft endstops on/off
+ if(gcode->has_letter('S')) {
+ soft_endstop_enabled= gcode->get_uint('S') == 1;
+ }else{
+ gcode->stream->printf("Soft endstops are %s", soft_endstop_enabled ? "Enabled" : "Disabled");
+ for (int i = X_AXIS; i <= Z_AXIS; ++i) {
+ if(isnan(soft_endstop_min[i])) {
+ gcode->stream->printf(",%c min is disabled", 'X'+i);
+ }
+ if(isnan(soft_endstop_max[i])) {
+ gcode->stream->printf(",%c max is disabled", 'X'+i);
+ }
+ if(!is_homed(i)) {
+ gcode->stream->printf(",%c axis is not homed", 'X'+i);
+ }
+ }
+ gcode->stream->printf("\n");
+ }
+ break;
+
case 220: // M220 - speed override percentage
if (gcode->has_letter('S')) {
float factor = gcode->get_value('S');
gcode->stream->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed mm/sec:\nM205 X%1.5f Z%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, isnan(THEKERNEL->planner->z_junction_deviation)?-1:THEKERNEL->planner->z_junction_deviation, THEKERNEL->planner->minimum_planner_speed);
- gcode->stream->printf(";Max cartesian feedrates in mm/sec:\nM203 X%1.5f Y%1.5f Z%1.5f\n", this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS]);
+ gcode->stream->printf(";Max cartesian feedrates in mm/sec:\nM203 X%1.5f Y%1.5f Z%1.5f S%1.5f\n", this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS], this->max_speed);
gcode->stream->printf(";Max actuator feedrates in mm/sec:\nM203.1 ");
for (int i = 0; i < n_motors; ++i) {
// save wcs_offsets and current_wcs
// TODO this may need to be done whenever they change to be compliant
- gcode->stream->printf(";WCS settings\n");
- gcode->stream->printf("%s\n", wcs2gcode(current_wcs).c_str());
- int n = 1;
- for(auto &i : wcs_offsets) {
- if(i != wcs_t(0, 0, 0)) {
- float x, y, z;
- std::tie(x, y, z) = i;
- gcode->stream->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n, x, y, z, wcs2gcode(n-1).c_str());
+ if(save_g54) {
+ gcode->stream->printf(";WCS settings\n");
+ gcode->stream->printf("%s\n", wcs2gcode(current_wcs).c_str());
+ int n = 1;
+ for(auto &i : wcs_offsets) {
+ if(i != wcs_t(0, 0, 0)) {
+ float x, y, z;
+ std::tie(x, y, z) = i;
+ gcode->stream->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n, x, y, z, wcs2gcode(n-1).c_str());
+ }
+ ++n;
}
- ++n;
}
if(save_g92) {
// linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
break;
}
+ // needed to act as start of next arc command
+ memcpy(arc_milestone, target, sizeof(arc_milestone));
+
if(moved) {
// set machine_position to the calculated target
memcpy(machine_position, target, n_motors*sizeof(float));
compensationTransform(transformed_target, false);
}
+ // check soft endstops only for homed axis that are enabled
+ if(soft_endstop_enabled) {
+ for (int i = 0; i <= Z_AXIS; ++i) {
+ if(!is_homed(i)) continue;
+ if( (!isnan(soft_endstop_min[i]) && transformed_target[i] < soft_endstop_min[i]) || (!isnan(soft_endstop_max[i]) && transformed_target[i] > soft_endstop_max[i]) ) {
+ if(soft_endstop_halt) {
+ if(THEKERNEL->is_grbl_mode()) {
+ THEKERNEL->streams->printf("error:");
+ }else{
+ THEKERNEL->streams->printf("Error: ");
+ }
+
+ THEKERNEL->streams->printf("Soft Endstop %c was exceeded - reset or $X or M999 required\n", i+'X');
+ THEKERNEL->call_event(ON_HALT, nullptr);
+ return false;
+
+ //} else if(soft_endstop_truncate) {
+ // TODO VERY hard to do need to go back and change the target, and calculate intercept with the edge
+ // and store all preceding vectors that have on eor more points ourtside of bounds so we can create a propper clip against the boundaries
+
+ } else {
+ // ignore it
+ if(THEKERNEL->is_grbl_mode()) {
+ THEKERNEL->streams->printf("error:");
+ }else{
+ THEKERNEL->streams->printf("Error: ");
+ }
+ THEKERNEL->streams->printf("Soft Endstop %c was exceeded - entire move ignored\n", i+'X');
+ return false;
+ }
+ }
+ }
+ }
+
+
bool move= false;
float sos= 0; // sum of squares for just primary axis (XYZ usually)
// as the last milestone won't be updated we do not actually lose any moves as they will be accounted for in the next move
if(!auxilliary_move && distance < 0.00001F) return false;
-
if(!auxilliary_move) {
for (size_t i = X_AXIS; i < N_PRIMARY_AXIS; i++) {
// find distance unit vector for primary axis only
rate_mm_s *= ( max_speeds[i] / axis_speed );
}
}
+
+ if(this->max_speed > 0 && rate_mm_s > this->max_speed) {
+ rate_mm_s= this->max_speed;
+ }
}
// find actuator position given the machine position, use actual adjusted target
return false;
}
- // Scary math
- float center_axis0 = this->machine_position[this->plane_axis_0] + offset[this->plane_axis_0];
- float center_axis1 = this->machine_position[this->plane_axis_1] + offset[this->plane_axis_1];
- float linear_travel = target[this->plane_axis_2] - this->machine_position[this->plane_axis_2];
- float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
+ // Scary math.
+ // We need to use arc_milestone here to get accurate arcs as previous machine_position may have been skipped due to small movements
+ float center_axis0 = this->arc_milestone[this->plane_axis_0] + offset[this->plane_axis_0];
+ float center_axis1 = this->arc_milestone[this->plane_axis_1] + offset[this->plane_axis_1];
+ float linear_travel = target[this->plane_axis_2] - this->arc_milestone[this->plane_axis_2];
+ float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to start position
float r_axis1 = -offset[this->plane_axis_1];
- float rt_axis0 = target[this->plane_axis_0] - center_axis0;
- float rt_axis1 = target[this->plane_axis_1] - center_axis1;
-
- // Patch from GRBL Firmware - Christoph Baumann 04072015
- // CCW angle between position and target from circle center. Only one atan2() trig computation required.
- float angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
- if (plane_axis_2 == Y_AXIS) { is_clockwise = !is_clockwise; } //Math for XZ plane is reverse of other 2 planes
- if (is_clockwise) { // Correct atan2 output per direction
- if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= (2 * PI); }
+ float rt_axis0 = target[this->plane_axis_0] - this->arc_milestone[this->plane_axis_0] - offset[this->plane_axis_0]; // Radius vector from center to target position
+ float rt_axis1 = target[this->plane_axis_1] - this->arc_milestone[this->plane_axis_1] - offset[this->plane_axis_1];
+ float angular_travel = 0;
+ //check for condition where atan2 formula will fail due to everything canceling out exactly
+ if((this->arc_milestone[this->plane_axis_0]==target[this->plane_axis_0]) && (this->arc_milestone[this->plane_axis_1]==target[this->plane_axis_1])) {
+ if (is_clockwise) { // set angular_travel to -2pi for a clockwise full circle
+ angular_travel = (-2 * PI);
+ } else { // set angular_travel to 2pi for a counterclockwise full circle
+ angular_travel = (2 * PI);
+ }
} else {
- if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += (2 * PI); }
+ // Patch from GRBL Firmware - Christoph Baumann 04072015
+ // CCW angle between position and target from circle center. Only one atan2() trig computation required.
+ // Only run if not a full circle or angular travel will incorrectly result in 0.0f
+ angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
+ if (plane_axis_2 == Y_AXIS) { is_clockwise = !is_clockwise; } //Math for XZ plane is reverse of other 2 planes
+ if (is_clockwise) { // adjust angular_travel to be in the range of -2pi to 0 for clockwise arcs
+ if (angular_travel > 0) { angular_travel -= (2 * PI); }
+ } else { // adjust angular_travel to be in the range of 0 to 2pi for counterclockwise arcs
+ if (angular_travel < 0) { angular_travel += (2 * PI); }
+ }
}
// Find the distance for this gcode
{
return THEKERNEL->gcode_dispatch->get_modal_command() == 0 ? seek_rate : feed_rate;
}
+
+bool Robot::is_homed(uint8_t i) const
+{
+ if(i >= 3) return false; // safety
+
+ // if we are homing we ignore soft endstops so return false
+ bool homing;
+ bool ok = PublicData::get_value(endstops_checksum, get_homing_status_checksum, 0, &homing);
+ if(!ok || homing) return false;
+
+ // check individual axis homing status
+ bool homed[3];
+ ok = PublicData::get_value(endstops_checksum, get_homed_status_checksum, 0, homed);
+ if(!ok) return false;
+ return homed[i];
+}
HCoop / clinton / Smoothieware.git / blobdiff