#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 = 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) );
-
}
void Robot::on_config_reload(void* argument){
}
- this->feed_rate = THEKERNEL->config->value(default_feed_rate_checksum )->by_default(100 )->as_number() / 60;
- this->seek_rate = THEKERNEL->config->value(default_seek_rate_checksum )->by_default(100 )->as_number() / 60;
- this->mm_per_line_segment = THEKERNEL->config->value(mm_per_line_segment_checksum )->by_default(0.0f )->as_number();
+ 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 )->as_number();
- this->max_speeds[Y_AXIS] = THEKERNEL->config->value(y_axis_max_speed_checksum )->by_default(60000 )->as_number();
- this->max_speeds[Z_AXIS] = THEKERNEL->config->value(z_axis_max_speed_checksum )->by_default(300 )->as_number();
- this->alpha_step_pin.from_string( THEKERNEL->config->value(alpha_step_pin_checksum )->by_default("2.0" )->as_string())->as_output();
- this->alpha_dir_pin.from_string( THEKERNEL->config->value(alpha_dir_pin_checksum )->by_default("0.5" )->as_string())->as_output();
- this->alpha_en_pin.from_string( THEKERNEL->config->value(alpha_en_pin_checksum )->by_default("0.4" )->as_string())->as_output();
- this->beta_step_pin.from_string( THEKERNEL->config->value(beta_step_pin_checksum )->by_default("2.1" )->as_string())->as_output();
- this->gamma_step_pin.from_string( THEKERNEL->config->value(gamma_step_pin_checksum )->by_default("2.2" )->as_string())->as_output();
- this->gamma_dir_pin.from_string( THEKERNEL->config->value(gamma_dir_pin_checksum )->by_default("0.20" )->as_string())->as_output();
- this->gamma_en_pin.from_string( THEKERNEL->config->value(gamma_en_pin_checksum )->by_default("0.19" )->as_string())->as_output();
- this->beta_dir_pin.from_string( THEKERNEL->config->value(beta_dir_pin_checksum )->by_default("0.11" )->as_string())->as_output();
- this->beta_en_pin.from_string( THEKERNEL->config->value(beta_en_pin_checksum )->by_default("0.10" )->as_string())->as_output();
-
+ 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->second_element_is(speed_override_percent_checksum)) {
static float return_data;
- return_data= 100*this->seconds_per_minute/60;
+ 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 float 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]);
+ 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();
// NOTE do not use this while printing!
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(float)*3); // current_position[] = last_milestone[];
- this->arm_solution->millimeters_to_steps(this->current_position, THEKERNEL->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){
- float 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, THEKERNEL->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 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'))
{
- float acc= gcode->get_value('S') * 60 * 60; // mm/min^2
+ // 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.0)
- acc = 1.0;
+ if (acc < 1.0F)
+ acc = 1.0F;
THEKERNEL->planner->acceleration= acc;
}
break;
{
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 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(";Acceleration mm/sec^2:\nM204 S%1.5f\n", THEKERNEL->planner->acceleration/3600);
+ 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++) {
+ if(c == 'S') continue; // used for segments per second
float v;
bool supported= arm_solution->get_optional(c, &v); // retrieve current 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;
-
}
}
//Get parameters
float target[3], offset[3];
- clear_vector(target); clear_vector(offset);
+ 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(float)*3); // this->position[] = target[];
+ memcpy(this->last_milestone, target, sizeof(this->last_milestone)); // this->position[] = target[];
}
// Reset the position for all axes ( used in homing and G92 stuff )
void Robot::reset_axis_position(float position, int axis) {
- this->last_milestone[axis] = this->current_position[axis] = position;
- this->arm_solution->millimeters_to_steps(this->current_position, THEKERNEL->planner->position);
-}
+ this->last_milestone[axis] = position;
+ float actuator_pos[3];
+ arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
-// Convert target from millimeters to steps, and append this to the planner
-void Robot::append_milestone( float target[], float rate ){
- int steps[3]; //Holds the result of the conversion
+ for (int i = 0; i < 3; i++)
+ actuators[i]->change_last_milestone(actuator_pos[i]);
+}
- // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
- this->arm_solution->millimeters_to_steps( target, steps );
+// Convert target from millimeters to steps, and append this to the planner
+void Robot::append_milestone( float target[], float rate_mm_s )
+{
float deltas[3];
- for(int axis=X_AXIS;axis<=Z_AXIS;axis++){deltas[axis]=target[axis]-this->last_milestone[axis];}
+ float unit_vec[3];
+ float actuator_pos[3];
+ float millimeters_of_travel;
+
+ // 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
- float millimeters_of_travel = sqrtf( 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 ){
- float 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
- THEKERNEL->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(float)*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, float target[], float rate ){
+void Robot::append_line(Gcode* gcode, float target[], float rate_mm_s ){
// Find out the distance for this gcode
- gcode->millimeters_of_travel = sqrtf( 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 ){
- // an extruder only move means we stopped so we need to tell planner that previous speed and unitvector are zero
- THEKERNEL->planner->previous_nominal_speed = 0;
- clear_vector_float(THEKERNEL->planner->previous_unit_vec);
+ 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
- float temp_target[3];
- //Initialize axes
- memcpy( temp_target, this->current_position, sizeof(float)*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();
void Robot::append_arc(Gcode* gcode, float target[], float offset[], float radius, bool is_clockwise ){
// Scary math
- float center_axis0 = this->current_position[this->plane_axis_0] + offset[this->plane_axis_0];
- float center_axis1 = this->current_position[this->plane_axis_1] + offset[this->plane_axis_1];
- float linear_travel = target[this->plane_axis_2] - this->current_position[this->plane_axis_2];
+ 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;
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 );
This is important when there are successive arc motions.
*/
// Vector rotation matrix values
- float cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
+ float cos_T = 1-0.5F*theta_per_segment*theta_per_segment; // Small angle approximation
float sin_T = theta_per_segment;
float arc_target[3];
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)
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
HCoop / clinton / Smoothieware.git / blobdiff