#include "libs/Module.h"
#include "libs/Kernel.h"
+
+#include <math.h>
#include <string>
using std::string;
-#include <math.h>
+
#include "Planner.h"
#include "Conveyor.h"
#include "Robot.h"
-#include "libs/nuts_bolts.h"
-#include "libs/Pin.h"
-#include "libs/StepperMotor.h"
-#include "../communication/utils/Gcode.h"
+#include "nuts_bolts.h"
+#include "Pin.h"
+#include "StepperMotor.h"
+#include "Gcode.h"
#include "PublicDataRequest.h"
#include "arm_solutions/BaseSolution.h"
#include "arm_solutions/CartesianSolution.h"
#include "arm_solutions/RostockSolution.h"
#include "arm_solutions/JohannKosselSolution.h"
#include "arm_solutions/HBotSolution.h"
-
-#define default_seek_rate_checksum CHECKSUM("default_seek_rate")
-#define default_feed_rate_checksum CHECKSUM("default_feed_rate")
-#define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
-#define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
-#define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
-#define arc_correction_checksum CHECKSUM("arc_correction")
-#define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
-#define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
-#define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
+#include "StepTicker.h"
+
+#define default_seek_rate_checksum CHECKSUM("default_seek_rate")
+#define default_feed_rate_checksum CHECKSUM("default_feed_rate")
+#define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
+#define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
+#define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
+#define arc_correction_checksum CHECKSUM("arc_correction")
+#define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
+#define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
+#define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
// arm solutions
-#define arm_solution_checksum CHECKSUM("arm_solution")
-#define cartesian_checksum CHECKSUM("cartesian")
-#define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
-#define rostock_checksum CHECKSUM("rostock")
-#define delta_checksum CHECKSUM("delta")
-#define hbot_checksum CHECKSUM("hbot")
-#define corexy_checksum CHECKSUM("corexy")
-#define kossel_checksum CHECKSUM("kossel")
+#define arm_solution_checksum CHECKSUM("arm_solution")
+#define cartesian_checksum CHECKSUM("cartesian")
+#define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
+#define rostock_checksum CHECKSUM("rostock")
+#define delta_checksum CHECKSUM("delta")
+#define hbot_checksum CHECKSUM("hbot")
+#define corexy_checksum CHECKSUM("corexy")
+#define kossel_checksum CHECKSUM("kossel")
+
+// stepper motor stuff
+#define alpha_step_pin_checksum CHECKSUM("alpha_step_pin")
+#define beta_step_pin_checksum CHECKSUM("beta_step_pin")
+#define gamma_step_pin_checksum CHECKSUM("gamma_step_pin")
+#define alpha_dir_pin_checksum CHECKSUM("alpha_dir_pin")
+#define beta_dir_pin_checksum CHECKSUM("beta_dir_pin")
+#define gamma_dir_pin_checksum CHECKSUM("gamma_dir_pin")
+#define alpha_en_pin_checksum CHECKSUM("alpha_en_pin")
+#define beta_en_pin_checksum CHECKSUM("beta_en_pin")
+#define gamma_en_pin_checksum CHECKSUM("gamma_en_pin")
+
+#define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
+#define beta_steps_per_mm_checksum CHECKSUM("beta_steps_per_mm")
+#define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
+
+#define alpha_max_rate_checksum CHECKSUM("alpha_max_rate")
+#define beta_max_rate_checksum CHECKSUM("beta_max_rate")
+#define gamma_max_rate_checksum CHECKSUM("gamma_max_rate")
+
+
+// new-style actuator stuff
+#define actuator_checksum CHEKCSUM("actuator")
+
+#define step_pin_checksum CHECKSUM("step_pin")
+#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 alpha_checksum CHECKSUM("alpha")
+#define beta_checksum CHECKSUM("beta")
+#define gamma_checksum CHECKSUM("gamma")
+
// 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
// It takes care of cutting arcs into segments, same thing for line that are too long
this->absolute_mode = true;
this->motion_mode = MOTION_MODE_SEEK;
this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
- clear_vector(this->current_position);
clear_vector(this->last_milestone);
this->arm_solution = NULL;
- seconds_per_minute = 60.0;
+ seconds_per_minute = 60.0F;
}
//Called when the module has just been loaded
// Configuration
this->on_config_reload(this);
-
- // Make our 3 StepperMotors
- this->alpha_stepper_motor = this->kernel->step_ticker->add_stepper_motor( new StepperMotor(&alpha_step_pin,&alpha_dir_pin,&alpha_en_pin) );
- this->beta_stepper_motor = this->kernel->step_ticker->add_stepper_motor( new StepperMotor(&beta_step_pin, &beta_dir_pin, &beta_en_pin ) );
- this->gamma_stepper_motor = this->kernel->step_ticker->add_stepper_motor( new StepperMotor(&gamma_step_pin,&gamma_dir_pin,&gamma_en_pin) );
-
}
void Robot::on_config_reload(void* argument){
// To make adding those solution easier, they have their own, separate object.
// Here we read the config to find out which arm solution to use
if (this->arm_solution) delete this->arm_solution;
- int solution_checksum = get_checksum(this->kernel->config->value(arm_solution_checksum)->by_default("cartesian")->as_string());
+ int solution_checksum = get_checksum(THEKERNEL->config->value(arm_solution_checksum)->by_default("cartesian")->as_string());
// Note checksums are not const expressions when in debug mode, so don't use switch
if(solution_checksum == hbot_checksum || solution_checksum == corexy_checksum) {
- this->arm_solution = new HBotSolution(this->kernel->config);
+ this->arm_solution = new HBotSolution(THEKERNEL->config);
}else if(solution_checksum == rostock_checksum) {
- this->arm_solution = new RostockSolution(this->kernel->config);
+ this->arm_solution = new RostockSolution(THEKERNEL->config);
}else if(solution_checksum == kossel_checksum) {
- this->arm_solution = new JohannKosselSolution(this->kernel->config);
+ this->arm_solution = new JohannKosselSolution(THEKERNEL->config);
}else if(solution_checksum == delta_checksum) {
// place holder for now
- this->arm_solution = new RostockSolution(this->kernel->config);
+ this->arm_solution = new RostockSolution(THEKERNEL->config);
}else if(solution_checksum == rotatable_cartesian_checksum) {
- this->arm_solution = new RotatableCartesianSolution(this->kernel->config);
+ this->arm_solution = new RotatableCartesianSolution(THEKERNEL->config);
}else if(solution_checksum == cartesian_checksum) {
- this->arm_solution = new CartesianSolution(this->kernel->config);
+ this->arm_solution = new CartesianSolution(THEKERNEL->config);
}else{
- this->arm_solution = new CartesianSolution(this->kernel->config);
+ this->arm_solution = new CartesianSolution(THEKERNEL->config);
}
- this->feed_rate = this->kernel->config->value(default_feed_rate_checksum )->by_default(100 )->as_number() / 60;
- this->seek_rate = this->kernel->config->value(default_seek_rate_checksum )->by_default(100 )->as_number() / 60;
- this->mm_per_line_segment = this->kernel->config->value(mm_per_line_segment_checksum )->by_default(0.0 )->as_number();
- this->delta_segments_per_second = this->kernel->config->value(delta_segments_per_second_checksum )->by_default(0.0 )->as_number();
- this->mm_per_arc_segment = this->kernel->config->value(mm_per_arc_segment_checksum )->by_default(0.5 )->as_number();
- this->arc_correction = this->kernel->config->value(arc_correction_checksum )->by_default(5 )->as_number();
- this->max_speeds[X_AXIS] = this->kernel->config->value(x_axis_max_speed_checksum )->by_default(60000 )->as_number();
- this->max_speeds[Y_AXIS] = this->kernel->config->value(y_axis_max_speed_checksum )->by_default(60000 )->as_number();
- this->max_speeds[Z_AXIS] = this->kernel->config->value(z_axis_max_speed_checksum )->by_default(300 )->as_number();
- this->alpha_step_pin.from_string( this->kernel->config->value(alpha_step_pin_checksum )->by_default("2.0" )->as_string())->as_output();
- this->alpha_dir_pin.from_string( this->kernel->config->value(alpha_dir_pin_checksum )->by_default("0.5" )->as_string())->as_output();
- this->alpha_en_pin.from_string( this->kernel->config->value(alpha_en_pin_checksum )->by_default("0.4" )->as_string())->as_output();
- this->beta_step_pin.from_string( this->kernel->config->value(beta_step_pin_checksum )->by_default("2.1" )->as_string())->as_output();
- this->gamma_step_pin.from_string( this->kernel->config->value(gamma_step_pin_checksum )->by_default("2.2" )->as_string())->as_output();
- this->gamma_dir_pin.from_string( this->kernel->config->value(gamma_dir_pin_checksum )->by_default("0.20" )->as_string())->as_output();
- this->gamma_en_pin.from_string( this->kernel->config->value(gamma_en_pin_checksum )->by_default("0.19" )->as_string())->as_output();
- this->beta_dir_pin.from_string( this->kernel->config->value(beta_dir_pin_checksum )->by_default("0.11" )->as_string())->as_output();
- this->beta_en_pin.from_string( this->kernel->config->value(beta_en_pin_checksum )->by_default("0.10" )->as_string())->as_output();
-
+ this->feed_rate = THEKERNEL->config->value(default_feed_rate_checksum )->by_default( 100.0F)->as_number();
+ this->seek_rate = THEKERNEL->config->value(default_seek_rate_checksum )->by_default( 100.0F)->as_number();
+ this->mm_per_line_segment = THEKERNEL->config->value(mm_per_line_segment_checksum )->by_default( 0.0F)->as_number();
+ this->delta_segments_per_second = THEKERNEL->config->value(delta_segments_per_second_checksum )->by_default(0.0f )->as_number();
+ this->mm_per_arc_segment = THEKERNEL->config->value(mm_per_arc_segment_checksum )->by_default( 0.5f)->as_number();
+ this->arc_correction = THEKERNEL->config->value(arc_correction_checksum )->by_default( 5 )->as_number();
+
+ 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;
+
+ Pin alpha_step_pin;
+ Pin alpha_dir_pin;
+ Pin alpha_en_pin;
+ Pin beta_step_pin;
+ Pin beta_dir_pin;
+ Pin beta_en_pin;
+ Pin gamma_step_pin;
+ Pin gamma_dir_pin;
+ Pin gamma_en_pin;
+
+ alpha_step_pin.from_string( THEKERNEL->config->value(alpha_step_pin_checksum )->by_default("2.0" )->as_string())->as_output();
+ alpha_dir_pin.from_string( THEKERNEL->config->value(alpha_dir_pin_checksum )->by_default("0.5" )->as_string())->as_output();
+ alpha_en_pin.from_string( THEKERNEL->config->value(alpha_en_pin_checksum )->by_default("0.4" )->as_string())->as_output();
+ beta_step_pin.from_string( THEKERNEL->config->value(beta_step_pin_checksum )->by_default("2.1" )->as_string())->as_output();
+ beta_dir_pin.from_string( THEKERNEL->config->value(beta_dir_pin_checksum )->by_default("0.11" )->as_string())->as_output();
+ beta_en_pin.from_string( THEKERNEL->config->value(beta_en_pin_checksum )->by_default("0.10" )->as_string())->as_output();
+ gamma_step_pin.from_string( THEKERNEL->config->value(gamma_step_pin_checksum )->by_default("2.2" )->as_string())->as_output();
+ gamma_dir_pin.from_string( THEKERNEL->config->value(gamma_dir_pin_checksum )->by_default("0.20" )->as_string())->as_output();
+ gamma_en_pin.from_string( THEKERNEL->config->value(gamma_en_pin_checksum )->by_default("0.19" )->as_string())->as_output();
+
+ float steps_per_mm[3] = {
+ THEKERNEL->config->value(alpha_steps_per_mm_checksum)->by_default( 80.0F)->as_number(),
+ THEKERNEL->config->value(beta_steps_per_mm_checksum )->by_default( 80.0F)->as_number(),
+ THEKERNEL->config->value(gamma_steps_per_mm_checksum)->by_default(2560.0F)->as_number(),
+ };
+
+ // TODO: delete or detect old steppermotors
+ // Make our 3 StepperMotors
+ this->alpha_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(alpha_step_pin, alpha_dir_pin, alpha_en_pin) );
+ this->beta_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(beta_step_pin, beta_dir_pin, beta_en_pin ) );
+ this->gamma_stepper_motor = THEKERNEL->step_ticker->add_stepper_motor( new StepperMotor(gamma_step_pin, gamma_dir_pin, gamma_en_pin) );
+
+ alpha_stepper_motor->change_steps_per_mm(steps_per_mm[0]);
+ beta_stepper_motor->change_steps_per_mm(steps_per_mm[1]);
+ gamma_stepper_motor->change_steps_per_mm(steps_per_mm[2]);
+
+ alpha_stepper_motor->max_rate = THEKERNEL->config->value(alpha_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F;
+ beta_stepper_motor->max_rate = THEKERNEL->config->value(beta_max_rate_checksum )->by_default(30000.0F)->as_number() / 60.0F;
+ gamma_stepper_motor->max_rate = THEKERNEL->config->value(gamma_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F;
+
+ actuators.clear();
+ actuators.push_back(alpha_stepper_motor);
+ actuators.push_back(beta_stepper_motor);
+ actuators.push_back(gamma_stepper_motor);
+
+ // initialise actuator positions to current cartesian position (X0 Y0 Z0)
+ // so the first move can be correct if homing is not performed
+ float actuator_pos[3];
+ arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
+ for (int i = 0; i < 3; i++)
+ actuators[i]->change_last_milestone(actuator_pos[i]);
}
void Robot::on_get_public_data(void* argument){
if(!pdr->starts_with(robot_checksum)) return;
if(pdr->second_element_is(speed_override_percent_checksum)) {
- static double return_data;
- return_data= 100*this->seconds_per_minute/60;
+ static float return_data;
+ return_data = 100.0F * 60.0F / seconds_per_minute;
pdr->set_data_ptr(&return_data);
pdr->set_taken();
}else if(pdr->second_element_is(current_position_checksum)) {
- static double return_data[3];
- return_data[0]= from_millimeters(this->current_position[0]);
- return_data[1]= from_millimeters(this->current_position[1]);
- return_data[2]= from_millimeters(this->current_position[2]);
+ static float return_data[3];
+ return_data[0]= from_millimeters(this->last_milestone[0]);
+ return_data[1]= from_millimeters(this->last_milestone[1]);
+ return_data[2]= from_millimeters(this->last_milestone[2]);
pdr->set_data_ptr(&return_data);
pdr->set_taken();
if(pdr->second_element_is(speed_override_percent_checksum)) {
// NOTE do not use this while printing!
- double t= *static_cast<double*>(pdr->get_data_ptr());
+ float t= *static_cast<float*>(pdr->get_data_ptr());
// enforce minimum 10% speed
- if (t < 10.0) t= 10.0;
+ if (t < 10.0F) t= 10.0F;
- this->seconds_per_minute= t * 0.6;
+ this->seconds_per_minute = t / 0.6F; // t * 60 / 100
pdr->set_taken();
}
}
this->last_milestone[letter-'X'] = this->to_millimeters(gcode->get_value(letter));
}
}
- memcpy(this->current_position, this->last_milestone, sizeof(double)*3); // current_position[] = last_milestone[];
- this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
+
+ // TODO: handle any number of actuators
+ float actuator_pos[3];
+ arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
+
+ for (int i = 0; i < 3; i++)
+ actuators[i]->change_last_milestone(actuator_pos[i]);
+
gcode->mark_as_taken();
- return; // TODO: Wait until queue empty
+ return;
}
}
}else if( gcode->has_m){
- double steps[3];
switch( gcode->m ){
case 92: // M92 - set steps per mm
- this->arm_solution->get_steps_per_millimeter(steps);
if (gcode->has_letter('X'))
- steps[0] = this->to_millimeters(gcode->get_value('X'));
+ actuators[0]->change_steps_per_mm(this->to_millimeters(gcode->get_value('X')));
if (gcode->has_letter('Y'))
- steps[1] = this->to_millimeters(gcode->get_value('Y'));
+ actuators[1]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Y')));
if (gcode->has_letter('Z'))
- steps[2] = this->to_millimeters(gcode->get_value('Z'));
+ actuators[2]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Z')));
if (gcode->has_letter('F'))
seconds_per_minute = gcode->get_value('F');
- this->arm_solution->set_steps_per_millimeter(steps);
- // update current position in steps
- this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
- gcode->stream->printf("X:%g Y:%g Z:%g F:%g ", steps[0], steps[1], steps[2], seconds_per_minute);
+
+ gcode->stream->printf("X:%g Y:%g Z:%g F:%g ", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm, seconds_per_minute);
gcode->add_nl = true;
gcode->mark_as_taken();
return;
case 114: gcode->stream->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
- from_millimeters(this->current_position[0]),
- from_millimeters(this->current_position[1]),
- from_millimeters(this->current_position[2]));
+ from_millimeters(this->last_milestone[0]),
+ from_millimeters(this->last_milestone[1]),
+ from_millimeters(this->last_milestone[2]));
gcode->add_nl = true;
gcode->mark_as_taken();
return;
- // TODO I'm not sure if the following is safe to do here, or should it go on the block queue?
- // case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
- // gcode->mark_as_taken();
- // if (gcode->has_letter('S'))
- // {
- // double acc= gcode->get_value('S') * 60 * 60; // mm/min^2
- // // enforce minimum
- // if (acc < 1.0)
- // acc = 1.0;
- // this->kernel->planner->acceleration= acc;
- // }
- // break;
+ case 203: // M203 Set maximum feedrates in mm/sec
+ if (gcode->has_letter('X'))
+ this->max_speeds[X_AXIS]= gcode->get_value('X');
+ if (gcode->has_letter('Y'))
+ this->max_speeds[Y_AXIS]= gcode->get_value('Y');
+ if (gcode->has_letter('Z'))
+ this->max_speeds[Z_AXIS]= gcode->get_value('Z');
+ if (gcode->has_letter('A'))
+ alpha_stepper_motor->max_rate= gcode->get_value('A');
+ if (gcode->has_letter('B'))
+ beta_stepper_motor->max_rate= gcode->get_value('B');
+ if (gcode->has_letter('C'))
+ gamma_stepper_motor->max_rate= gcode->get_value('C');
+
+ gcode->stream->printf("X:%g Y:%g Z:%g A:%g B:%g C:%g ",
+ this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS],
+ alpha_stepper_motor->max_rate, beta_stepper_motor->max_rate, gamma_stepper_motor->max_rate);
+ gcode->add_nl = true;
+ gcode->mark_as_taken();
+ break;
+
+ case 204: // M204 Snnn - set acceleration to nnn, NB only Snnn is currently supported
+ gcode->mark_as_taken();
+
+ if (gcode->has_letter('S'))
+ {
+ // TODO for safety so it applies only to following gcodes, maybe a better way to do this?
+ THEKERNEL->conveyor->wait_for_empty_queue();
+ float acc= gcode->get_value('S'); // mm/s^2
+ // enforce minimum
+ if (acc < 1.0F)
+ acc = 1.0F;
+ THEKERNEL->planner->acceleration= acc;
+ }
+ break;
+
+ case 205: // M205 Xnnn - set junction deviation Snnn - Set minimum planner speed
+ gcode->mark_as_taken();
+ if (gcode->has_letter('X'))
+ {
+ float jd= gcode->get_value('X');
+ // enforce minimum
+ if (jd < 0.0F)
+ jd = 0.0F;
+ THEKERNEL->planner->junction_deviation= jd;
+ }
+ if (gcode->has_letter('S'))
+ {
+ float mps= gcode->get_value('S');
+ // enforce minimum
+ if (mps < 0.0F)
+ mps = 0.0F;
+ THEKERNEL->planner->minimum_planner_speed= mps;
+ }
+ break;
case 220: // M220 - speed override percentage
gcode->mark_as_taken();
if (gcode->has_letter('S'))
{
- double factor = gcode->get_value('S');
+ float factor = gcode->get_value('S');
// enforce minimum 10% speed
- if (factor < 10.0)
- factor = 10.0;
- seconds_per_minute = factor * 0.6;
+ if (factor < 10.0F)
+ factor = 10.0F;
+ // enforce maximum 10x speed
+ if (factor > 1000.0F)
+ factor = 1000.0F;
+
+ seconds_per_minute = 6000.0F / factor;
}
break;
case 400: // wait until all moves are done up to this point
gcode->mark_as_taken();
- this->kernel->conveyor->wait_for_empty_queue();
+ THEKERNEL->conveyor->wait_for_empty_queue();
break;
case 500: // M500 saves some volatile settings to config override file
case 503: // M503 just prints the settings
- this->arm_solution->get_steps_per_millimeter(steps);
- gcode->stream->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", steps[0], steps[1], steps[2]);
+ gcode->stream->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", actuators[0]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm);
+ gcode->stream->printf(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL->planner->acceleration);
+ gcode->stream->printf(";X- Junction Deviation, S - Minimum Planner speed:\nM205 X%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, THEKERNEL->planner->minimum_planner_speed);
+ gcode->stream->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f A%1.5f B%1.5f C%1.5f\n",
+ this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS],
+ alpha_stepper_motor->max_rate, beta_stepper_motor->max_rate, gamma_stepper_motor->max_rate);
gcode->mark_as_taken();
break;
- case 665: // M665 set optional arm solution variables based on arm solution
+ case 665: // M665 set optional arm solution variables based on arm solution. NOTE these are not saved with M500
gcode->mark_as_taken();
- // the parameter args could be any letter so try each one
+ // the parameter args could be any letter except S so try each one
for(char c='A';c<='Z';c++) {
- double v;
+ if(c == 'S') continue; // used for segments per second
+ float v;
bool supported= arm_solution->get_optional(c, &v); // retrieve current value if supported
if(supported && gcode->has_letter(c)) { // set new value if supported
gcode->add_nl = true;
}
}
+ // set delta segments per second
+ if(gcode->has_letter('S')) {
+ this->delta_segments_per_second= gcode->get_value('S');
+ }
break;
-
}
}
return;
//Get parameters
- double target[3], offset[3];
- clear_vector(target); clear_vector(offset);
+ float target[3], offset[3];
+ clear_vector(offset);
- memcpy(target, this->current_position, sizeof(target)); //default to last target
+ memcpy(target, this->last_milestone, sizeof(target)); //default to last target
- for(char letter = 'I'; letter <= 'K'; letter++){ if( gcode->has_letter(letter) ){ offset[letter-'I'] = this->to_millimeters(gcode->get_value(letter)); } }
- for(char letter = 'X'; letter <= 'Z'; letter++){ if( gcode->has_letter(letter) ){ target[letter-'X'] = this->to_millimeters(gcode->get_value(letter)) + ( this->absolute_mode ? 0 : target[letter-'X']); } }
+ for(char letter = 'I'; letter <= 'K'; letter++){
+ if( gcode->has_letter(letter) ){
+ offset[letter-'I'] = this->to_millimeters(gcode->get_value(letter));
+ }
+ }
+ for(char letter = 'X'; letter <= 'Z'; letter++){
+ if( gcode->has_letter(letter) ){
+ target[letter-'X'] = this->to_millimeters(gcode->get_value(letter)) + ( this->absolute_mode ? 0 : target[letter-'X']);
+ }
+ }
if( gcode->has_letter('F') )
{
if( this->motion_mode == MOTION_MODE_SEEK )
- this->seek_rate = this->to_millimeters( gcode->get_value('F') ) / 60.0;
+ this->seek_rate = this->to_millimeters( gcode->get_value('F') );
else
- this->feed_rate = this->to_millimeters( gcode->get_value('F') ) / 60.0;
+ this->feed_rate = this->to_millimeters( gcode->get_value('F') );
}
//Perform any physical actions
case NEXT_ACTION_DEFAULT:
switch(this->motion_mode){
case MOTION_MODE_CANCEL: break;
- case MOTION_MODE_SEEK : this->append_line(gcode, target, this->seek_rate ); break;
- case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate ); break;
+ case MOTION_MODE_SEEK : this->append_line(gcode, target, this->seek_rate / seconds_per_minute ); break;
+ case MOTION_MODE_LINEAR: this->append_line(gcode, target, this->feed_rate / seconds_per_minute ); break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: this->compute_arc(gcode, offset, target ); break;
}
break;
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
- memcpy(this->current_position, target, sizeof(double)*3); // this->position[] = target[];
+ memcpy(this->last_milestone, target, sizeof(this->last_milestone)); // this->position[] = target[];
}
void Robot::distance_in_gcode_is_known(Gcode* gcode){
//If the queue is empty, execute immediatly, otherwise attach to the last added block
- if( this->kernel->conveyor->queue.size() == 0 ){
- this->kernel->call_event(ON_GCODE_EXECUTE, gcode );
- }else{
- Block* block = this->kernel->conveyor->queue.get_ref( this->kernel->conveyor->queue.size() - 1 );
- block->append_gcode(gcode);
- }
-
+ THEKERNEL->conveyor->append_gcode(gcode);
}
// Reset the position for all axes ( used in homing and G92 stuff )
-void Robot::reset_axis_position(double position, int axis) {
- this->last_milestone[axis] = this->current_position[axis] = position;
- this->arm_solution->millimeters_to_steps(this->current_position, this->kernel->planner->position);
+void Robot::reset_axis_position(float position, int axis) {
+ this->last_milestone[axis] = position;
+
+ float actuator_pos[3];
+ arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
+
+ for (int i = 0; i < 3; i++)
+ actuators[i]->change_last_milestone(actuator_pos[i]);
}
// Convert target from millimeters to steps, and append this to the planner
-void Robot::append_milestone( double target[], double rate ){
- int steps[3]; //Holds the result of the conversion
-
- // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
- this->arm_solution->millimeters_to_steps( target, steps );
+void Robot::append_milestone( float target[], float rate_mm_s )
+{
+ float deltas[3];
+ float unit_vec[3];
+ float actuator_pos[3];
+ float millimeters_of_travel;
- double deltas[3];
- for(int axis=X_AXIS;axis<=Z_AXIS;axis++){deltas[axis]=target[axis]-this->last_milestone[axis];}
+ // find distance moved by each axis
+ for (int axis = X_AXIS; axis <= Z_AXIS; axis++)
+ deltas[axis] = target[axis] - last_milestone[axis];
// Compute how long this move moves, so we can attach it to the block for later use
- double millimeters_of_travel = sqrt( pow( deltas[X_AXIS], 2 ) + pow( deltas[Y_AXIS], 2 ) + pow( deltas[Z_AXIS], 2 ) );
-
- // Do not move faster than the configured limits
- for(int axis=X_AXIS;axis<=Z_AXIS;axis++){
- if( this->max_speeds[axis] > 0 ){
- double axis_speed = ( fabs(deltas[axis]) / ( millimeters_of_travel / rate )) * seconds_per_minute;
- if( axis_speed > this->max_speeds[axis] ){
- rate = rate * ( this->max_speeds[axis] / axis_speed );
- }
+ millimeters_of_travel = sqrtf( pow( deltas[X_AXIS], 2 ) + pow( deltas[Y_AXIS], 2 ) + pow( deltas[Z_AXIS], 2 ) );
+
+ // find distance unit vector
+ for (int i = 0; i < 3; i++)
+ unit_vec[i] = deltas[i] / millimeters_of_travel;
+
+ // Do not move faster than the configured cartesian limits
+ for (int axis = X_AXIS; axis <= Z_AXIS; axis++)
+ {
+ if ( max_speeds[axis] > 0 )
+ {
+ float axis_speed = fabs(unit_vec[axis] * rate_mm_s);
+
+ if (axis_speed > max_speeds[axis])
+ rate_mm_s *= ( max_speeds[axis] / axis_speed );
}
}
+ // find actuator position given cartesian position
+ arm_solution->cartesian_to_actuator( target, actuator_pos );
+
+ // check per-actuator speed limits
+ for (int actuator = 0; actuator <= 2; actuator++)
+ {
+ float actuator_rate = fabs(actuator_pos[actuator] - actuators[actuator]->last_milestone_mm) * rate_mm_s / millimeters_of_travel;
+
+ if (actuator_rate > actuators[actuator]->max_rate)
+ rate_mm_s *= (actuators[actuator]->max_rate / actuator_rate);
+ }
+
// Append the block to the planner
- this->kernel->planner->append_block( steps, rate * seconds_per_minute, millimeters_of_travel, deltas );
+ THEKERNEL->planner->append_block( actuator_pos, rate_mm_s, millimeters_of_travel, unit_vec );
// Update the last_milestone to the current target for the next time we use last_milestone
- memcpy(this->last_milestone, target, sizeof(double)*3); // this->last_milestone[] = target[];
+ memcpy(this->last_milestone, target, sizeof(this->last_milestone)); // this->last_milestone[] = target[];
}
// Append a move to the queue ( cutting it into segments if needed )
-void Robot::append_line(Gcode* gcode, double target[], double rate ){
+void Robot::append_line(Gcode* gcode, float target[], float rate_mm_s ){
// Find out the distance for this gcode
- gcode->millimeters_of_travel = sqrt( pow( target[X_AXIS]-this->current_position[X_AXIS], 2 ) + pow( target[Y_AXIS]-this->current_position[Y_AXIS], 2 ) + pow( target[Z_AXIS]-this->current_position[Z_AXIS], 2 ) );
+ gcode->millimeters_of_travel = pow( target[X_AXIS]-this->last_milestone[X_AXIS], 2 ) + pow( target[Y_AXIS]-this->last_milestone[Y_AXIS], 2 ) + pow( target[Z_AXIS]-this->last_milestone[Z_AXIS], 2 );
// We ignore non-moves ( for example, extruder moves are not XYZ moves )
- if( gcode->millimeters_of_travel < 0.0001 ){ return; }
+ if( gcode->millimeters_of_travel < 1e-8F ){
+ return;
+ }
+
+ gcode->millimeters_of_travel = sqrtf(gcode->millimeters_of_travel);
// Mark the gcode as having a known distance
this->distance_in_gcode_is_known( gcode );
// In delta robots either mm_per_line_segment can be used OR delta_segments_per_second The latter is more efficient and avoids splitting fast long lines into very small segments, like initial z move to 0, it is what Johanns Marlin delta port does
uint16_t segments;
- if(this->delta_segments_per_second > 1.0) {
+ if(this->delta_segments_per_second > 1.0F) {
// enabled if set to something > 1, it is set to 0.0 by default
// segment based on current speed and requested segments per second
// the faster the travel speed the fewer segments needed
// NOTE rate is mm/sec and we take into account any speed override
- float seconds = 60.0/seconds_per_minute * gcode->millimeters_of_travel / rate;
+ float seconds = gcode->millimeters_of_travel / rate_mm_s;
segments= max(1, ceil(this->delta_segments_per_second * seconds));
// TODO if we are only moving in Z on a delta we don't really need to segment at all
}else{
- if(this->mm_per_line_segment == 0.0){
+ if(this->mm_per_line_segment == 0.0F){
segments= 1; // don't split it up
}else{
segments = ceil( gcode->millimeters_of_travel/ this->mm_per_line_segment);
}
}
- // A vector to keep track of the endpoint of each segment
- double temp_target[3];
- //Initialize axes
- memcpy( temp_target, this->current_position, sizeof(double)*3); // temp_target[] = this->current_position[];
-
- //For each segment
- for( int i=0; i<segments-1; i++ ){
- for(int axis=X_AXIS; axis <= Z_AXIS; axis++ ){ temp_target[axis] += ( target[axis]-this->current_position[axis] )/segments; }
- // Append the end of this segment to the queue
- this->append_milestone(temp_target, rate);
+ if (segments > 1)
+ {
+ // A vector to keep track of the endpoint of each segment
+ float segment_delta[3];
+ float segment_end[3];
+
+ // How far do we move each segment?
+ for (int i = X_AXIS; i <= Z_AXIS; i++)
+ segment_delta[i] = (target[i] - last_milestone[i]) / segments;
+
+ // segment 0 is already done - it's the end point of the previous move so we start at segment 1
+ // We always add another point after this loop so we stop at segments-1, ie i < segments
+ for (int i = 1; i < segments; i++)
+ {
+ for(int axis=X_AXIS; axis <= Z_AXIS; axis++ )
+ segment_end[axis] = last_milestone[axis] + segment_delta[axis];
+
+ // Append the end of this segment to the queue
+ this->append_milestone(segment_end, rate_mm_s);
+ }
}
// Append the end of this full move to the queue
- this->append_milestone(target, rate);
+ this->append_milestone(target, rate_mm_s);
+
+ // if adding these blocks didn't start executing, do that now
+ THEKERNEL->conveyor->ensure_running();
}
// Append an arc to the queue ( cutting it into segments as needed )
-void Robot::append_arc(Gcode* gcode, double target[], double offset[], double radius, bool is_clockwise ){
+void Robot::append_arc(Gcode* gcode, float target[], float offset[], float radius, bool is_clockwise ){
// Scary math
- double center_axis0 = this->current_position[this->plane_axis_0] + offset[this->plane_axis_0];
- double center_axis1 = this->current_position[this->plane_axis_1] + offset[this->plane_axis_1];
- double linear_travel = target[this->plane_axis_2] - this->current_position[this->plane_axis_2];
- double r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
- double r_axis1 = -offset[this->plane_axis_1];
- double rt_axis0 = target[this->plane_axis_0] - center_axis0;
- double rt_axis1 = target[this->plane_axis_1] - center_axis1;
+ float center_axis0 = this->last_milestone[this->plane_axis_0] + offset[this->plane_axis_0];
+ float center_axis1 = this->last_milestone[this->plane_axis_1] + offset[this->plane_axis_1];
+ float linear_travel = target[this->plane_axis_2] - this->last_milestone[this->plane_axis_2];
+ float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
+ 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;
// CCW angle between position and target from circle center. Only one atan2() trig computation required.
- double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
+ float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
if (angular_travel < 0) { angular_travel += 2*M_PI; }
if (is_clockwise) { angular_travel -= 2*M_PI; }
// Find the distance for this gcode
- gcode->millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
+ gcode->millimeters_of_travel = hypotf(angular_travel*radius, fabs(linear_travel));
// We don't care about non-XYZ moves ( for example the extruder produces some of those )
- if( gcode->millimeters_of_travel < 0.0001 ){ return; }
+ if( gcode->millimeters_of_travel < 0.0001F ){ return; }
// Mark the gcode as having a known distance
this->distance_in_gcode_is_known( gcode );
// Figure out how many segments for this gcode
uint16_t segments = floor(gcode->millimeters_of_travel/this->mm_per_arc_segment);
- double theta_per_segment = angular_travel/segments;
- double linear_per_segment = linear_travel/segments;
+ float theta_per_segment = angular_travel/segments;
+ float linear_per_segment = linear_travel/segments;
/* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
defined from the circle center to the initial position. Each line segment is formed by successive
vector rotations. This requires only two cos() and sin() computations to form the rotation
matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
- all double numbers are single precision on the Arduino. (True double precision will not have
+ all float numbers are single precision on the Arduino. (True float precision will not have
round off issues for CNC applications.) Single precision error can accumulate to be greater than
tool precision in some cases. Therefore, arc path correction is implemented.
This is important when there are successive arc motions.
*/
// Vector rotation matrix values
- double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
- double sin_T = theta_per_segment;
+ float cos_T = 1-0.5F*theta_per_segment*theta_per_segment; // Small angle approximation
+ float sin_T = theta_per_segment;
- double arc_target[3];
- double sin_Ti;
- double cos_Ti;
- double r_axisi;
+ float arc_target[3];
+ float sin_Ti;
+ float cos_Ti;
+ float r_axisi;
uint16_t i;
int8_t count = 0;
// Initialize the linear axis
- arc_target[this->plane_axis_2] = this->current_position[this->plane_axis_2];
+ arc_target[this->plane_axis_2] = this->last_milestone[this->plane_axis_2];
for (i = 1; i<segments; i++) { // Increment (segments-1)
} else {
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
- cos_Ti = cos(i*theta_per_segment);
- sin_Ti = sin(i*theta_per_segment);
+ cos_Ti = cosf(i*theta_per_segment);
+ sin_Ti = sinf(i*theta_per_segment);
r_axis0 = -offset[this->plane_axis_0]*cos_Ti + offset[this->plane_axis_1]*sin_Ti;
r_axis1 = -offset[this->plane_axis_0]*sin_Ti - offset[this->plane_axis_1]*cos_Ti;
count = 0;
arc_target[this->plane_axis_2] += linear_per_segment;
// Append this segment to the queue
- this->append_milestone(arc_target, this->feed_rate);
+ this->append_milestone(arc_target, this->feed_rate / seconds_per_minute);
}
// Ensure last segment arrives at target location.
- this->append_milestone(target, this->feed_rate);
+ this->append_milestone(target, this->feed_rate / seconds_per_minute);
}
// Do the math for an arc and add it to the queue
-void Robot::compute_arc(Gcode* gcode, double offset[], double target[]){
+void Robot::compute_arc(Gcode* gcode, float offset[], float target[]){
// Find the radius
- double radius = hypot(offset[this->plane_axis_0], offset[this->plane_axis_1]);
+ float radius = hypotf(offset[this->plane_axis_0], offset[this->plane_axis_1]);
// Set clockwise/counter-clockwise sign for mc_arc computations
bool is_clockwise = false;
}
-double Robot::theta(double x, double y){
- double t = atan(x/fabs(y));
+float Robot::theta(float x, float y){
+ float t = atanf(x/fabs(y));
if (y>0) {return(t);} else {if (t>0){return(M_PI-t);} else {return(-M_PI-t);}}
}