// It takes care of cutting arcs into segments, same thing for line that are too long
#define max(a,b) (((a) > (b)) ? (a) : (b))
-Robot::Robot(){
+Robot::Robot()
+{
this->inch_mode = false;
this->absolute_mode = true;
this->motion_mode = MOTION_MODE_SEEK;
clear_vector(this->last_milestone);
this->arm_solution = NULL;
seconds_per_minute = 60.0F;
- this->setToolOffset(0.0, 0.0, 0.0);
+ this->setToolOffset(0.0, 0.0, 0.0);
}
//Called when the module has just been loaded
-void Robot::on_module_loaded() {
+void Robot::on_module_loaded()
+{
register_for_event(ON_CONFIG_RELOAD);
this->register_for_event(ON_GCODE_RECEIVED);
this->register_for_event(ON_GET_PUBLIC_DATA);
this->on_config_reload(this);
}
-void Robot::on_config_reload(void* argument){
+void Robot::on_config_reload(void *argument)
+{
// Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
// While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
if(solution_checksum == hbot_checksum || solution_checksum == corexy_checksum) {
this->arm_solution = new HBotSolution(THEKERNEL->config);
- }else if(solution_checksum == rostock_checksum) {
+ } else if(solution_checksum == rostock_checksum) {
this->arm_solution = new RostockSolution(THEKERNEL->config);
- }else if(solution_checksum == kossel_checksum) {
+ } else if(solution_checksum == kossel_checksum) {
this->arm_solution = new JohannKosselSolution(THEKERNEL->config);
- }else if(solution_checksum == delta_checksum) {
+ } else if(solution_checksum == delta_checksum) {
// place holder for now
this->arm_solution = new RostockSolution(THEKERNEL->config);
- }else if(solution_checksum == rotatable_cartesian_checksum) {
+ } else if(solution_checksum == rotatable_cartesian_checksum) {
this->arm_solution = new RotatableCartesianSolution(THEKERNEL->config);
- }else if(solution_checksum == cartesian_checksum) {
+ } else if(solution_checksum == cartesian_checksum) {
this->arm_solution = new CartesianSolution(THEKERNEL->config);
- }else{
+ } else {
this->arm_solution = new CartesianSolution(THEKERNEL->config);
}
//this->clearToolOffset();
}
-void Robot::on_get_public_data(void* argument){
- PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
+void Robot::on_get_public_data(void *argument)
+{
+ PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
if(!pdr->starts_with(robot_checksum)) return;
pdr->set_data_ptr(&return_data);
pdr->set_taken();
- }else if(pdr->second_element_is(current_position_checksum)) {
+ } else if(pdr->second_element_is(current_position_checksum)) {
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]);
+ 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();
}
}
-void Robot::on_set_public_data(void* argument){
- PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
+void Robot::on_set_public_data(void *argument)
+{
+ PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
if(!pdr->starts_with(robot_checksum)) return;
if(pdr->second_element_is(speed_override_percent_checksum)) {
// NOTE do not use this while printing!
- float t= *static_cast<float*>(pdr->get_data_ptr());
+ float t = *static_cast<float *>(pdr->get_data_ptr());
// enforce minimum 10% speed
- if (t < 10.0F) t= 10.0F;
+ if (t < 10.0F) t = 10.0F;
this->seconds_per_minute = t / 0.6F; // t * 60 / 100
pdr->set_taken();
- }
- else if(pdr->second_element_is(current_position_checksum)) {
- float *t= static_cast<float*>(pdr->get_data_ptr());
- for (int i = 0; i < 3; i++){
+ } else if(pdr->second_element_is(current_position_checksum)) {
+ float *t = static_cast<float *>(pdr->get_data_ptr());
+ for (int i = 0; i < 3; i++) {
this->last_milestone[i] = this->to_millimeters(t[i]);
}
//A GCode has been received
//See if the current Gcode line has some orders for us
-void Robot::on_gcode_received(void * argument){
- Gcode* gcode = static_cast<Gcode*>(argument);
+void Robot::on_gcode_received(void *argument)
+{
+ Gcode *gcode = static_cast<Gcode *>(argument);
//Temp variables, constant properties are stored in the object
uint8_t next_action = NEXT_ACTION_DEFAULT;
this->motion_mode = -1;
- //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
- if( gcode->has_g){
- switch( gcode->g ){
+ //G-letter Gcodes are mostly what the Robot module is interrested in, other modules also catch the gcode event and do stuff accordingly
+ if( gcode->has_g) {
+ switch( gcode->g ) {
case 0: this->motion_mode = MOTION_MODE_SEEK; gcode->mark_as_taken(); break;
case 1: this->motion_mode = MOTION_MODE_LINEAR; gcode->mark_as_taken(); break;
case 2: this->motion_mode = MOTION_MODE_CW_ARC; gcode->mark_as_taken(); break;
case 90: this->absolute_mode = true; gcode->mark_as_taken(); break;
case 91: this->absolute_mode = false; gcode->mark_as_taken(); break;
case 92: {
- if(gcode->get_num_args() == 0){
+ if(gcode->get_num_args() == 0) {
clear_vector(this->last_milestone);
- }else{
- for (char letter = 'X'; letter <= 'Z'; letter++){
+ } else {
+ for (char letter = 'X'; letter <= 'Z'; letter++) {
if ( gcode->has_letter(letter) )
- this->last_milestone[letter-'X'] = this->to_millimeters(gcode->get_value(letter));
+ this->last_milestone[letter - 'X'] = this->to_millimeters(gcode->get_value(letter));
}
}
gcode->mark_as_taken();
return;
- }
- }
- }else if( gcode->has_m){
- switch( gcode->m ){
+ }
+ }
+ } else if( gcode->has_m) {
+ switch( gcode->m ) {
case 92: // M92 - set steps per mm
if (gcode->has_letter('X'))
actuators[0]->change_steps_per_mm(this->to_millimeters(gcode->get_value('X')));
gcode->add_nl = true;
gcode->mark_as_taken();
return;
- case 114:
- {
- char buf[32];
- int n= snprintf(buf, sizeof(buf), "C: X:%1.3f Y:%1.3f Z:%1.3f",
- from_millimeters(this->last_milestone[0]),
- from_millimeters(this->last_milestone[1]),
- from_millimeters(this->last_milestone[2]));
- gcode->txt_after_ok.append(buf, n);
- gcode->mark_as_taken();
- }
- return;
+ case 114: {
+ char buf[32];
+ int n = snprintf(buf, sizeof(buf), "C: X:%1.3f Y:%1.3f Z:%1.3f",
+ from_millimeters(this->last_milestone[0]),
+ from_millimeters(this->last_milestone[1]),
+ from_millimeters(this->last_milestone[2]));
+ gcode->txt_after_ok.append(buf, n);
+ gcode->mark_as_taken();
+ }
+ return;
case 203: // M203 Set maximum feedrates in mm/sec
if (gcode->has_letter('X'))
- this->max_speeds[X_AXIS]= gcode->get_value('X');
+ this->max_speeds[X_AXIS] = gcode->get_value('X');
if (gcode->has_letter('Y'))
- this->max_speeds[Y_AXIS]= gcode->get_value('Y');
+ this->max_speeds[Y_AXIS] = gcode->get_value('Y');
if (gcode->has_letter('Z'))
- this->max_speeds[Z_AXIS]= gcode->get_value('Z');
+ this->max_speeds[Z_AXIS] = gcode->get_value('Z');
if (gcode->has_letter('A'))
- alpha_stepper_motor->max_rate= gcode->get_value('A');
+ alpha_stepper_motor->max_rate = gcode->get_value('A');
if (gcode->has_letter('B'))
- beta_stepper_motor->max_rate= gcode->get_value('B');
+ beta_stepper_motor->max_rate = gcode->get_value('B');
if (gcode->has_letter('C'))
- gamma_stepper_motor->max_rate= gcode->get_value('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);
+ 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'))
- {
+ 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
+ float acc = gcode->get_value('S'); // mm/s^2
// enforce minimum
if (acc < 1.0F)
acc = 1.0F;
- THEKERNEL->planner->acceleration= acc;
+ 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');
+ 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;
+ THEKERNEL->planner->junction_deviation = jd;
}
- if (gcode->has_letter('S'))
- {
- float mps= gcode->get_value('S');
+ 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;
+ THEKERNEL->planner->minimum_planner_speed = mps;
}
break;
case 220: // M220 - speed override percentage
gcode->mark_as_taken();
- if (gcode->has_letter('S'))
- {
+ if (gcode->has_letter('S')) {
float factor = gcode->get_value('S');
// enforce minimum 10% speed
if (factor < 10.0F)
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);
+ 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);
// get or save any arm solution specific optional values
BaseSolution::arm_options_t options;
if(arm_solution->get_optional(options) && !options.empty()) {
gcode->stream->printf(";Optional arm solution specific settings:\nM665");
- for(auto& i : options) {
+ for(auto &i : options) {
gcode->stream->printf(" %c%1.4f", i.first, i.second);
}
gcode->stream->printf("\n");
// the parameter args could be any letter except S so ask solution what options it supports
BaseSolution::arm_options_t options;
if(arm_solution->get_optional(options)) {
- for(auto& i : options) {
+ for(auto &i : options) {
// foreach optional value
- char c= i.first;
+ char c = i.first;
if(gcode->has_letter(c)) { // set new value
- i.second= gcode->get_value(c);
+ i.second = gcode->get_value(c);
}
// print all current values of supported options
gcode->stream->printf("%c: %8.4f ", i.first, i.second);
// set delta segments per second, not saved by M500
if(gcode->has_letter('S')) {
- this->delta_segments_per_second= gcode->get_value('S');
+ this->delta_segments_per_second = gcode->get_value('S');
}
break;
}
if( this->motion_mode < 0)
return;
- //Get parameters
+ //Get parameters
float target[3], offset[3];
clear_vector(offset);
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 = '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']) + this->toolOffset[letter - 'X'];
+ 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']) + this->toolOffset[letter - 'X'];
}
}
- if( gcode->has_letter('F') )
- {
+ if( gcode->has_letter('F') ) {
if( this->motion_mode == MOTION_MODE_SEEK )
this->seek_rate = this->to_millimeters( gcode->get_value('F') );
else
}
//Perform any physical actions
- switch( next_action ){
+ switch( next_action ) {
case NEXT_ACTION_DEFAULT:
- switch(this->motion_mode){
+ switch(this->motion_mode) {
case MOTION_MODE_CANCEL: 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;
// We received a new gcode, and one of the functions
// determined the distance for that given gcode. So now we can attach this gcode to the right block
// and continue
-void Robot::distance_in_gcode_is_known(Gcode* gcode){
+void Robot::distance_in_gcode_is_known(Gcode *gcode)
+{
//If the queue is empty, execute immediatly, otherwise attach to the last added block
THEKERNEL->conveyor->append_gcode(gcode);
}
// Reset the position for all axes ( used in homing and G92 stuff )
-void Robot::reset_axis_position(float position, int axis) {
+void Robot::reset_axis_position(float position, int axis)
+{
this->last_milestone[axis] = position;
float actuator_pos[3];
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 )
- {
+ 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])
arm_solution->cartesian_to_actuator( target, actuator_pos );
// check per-actuator speed limits
- for (int actuator = 0; actuator <= 2; actuator++)
- {
+ 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)
}
// Append a move to the queue ( cutting it into segments if needed )
-void Robot::append_line(Gcode* gcode, float target[], float rate_mm_s ){
+void Robot::append_line(Gcode *gcode, float target[], float rate_mm_s )
+{
// Find out the distance for this gcode
- 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 );
+ 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 < 1e-8F ){
+ if( gcode->millimeters_of_travel < 1e-8F ) {
return;
}
// the faster the travel speed the fewer segments needed
// NOTE rate is mm/sec and we take into account any speed override
float seconds = gcode->millimeters_of_travel / rate_mm_s;
- segments= max(1, ceil(this->delta_segments_per_second * seconds));
+ 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.0F){
- segments= 1; // don't split it up
- }else{
- segments = ceil( gcode->millimeters_of_travel/ this->mm_per_line_segment);
+ } else {
+ 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);
}
}
- if (segments > 1)
- {
+ if (segments > 1) {
// A vector to keep track of the endpoint of each segment
float segment_delta[3];
float segment_end[3];
// 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++ )
+ 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
// Append an arc to the queue ( cutting it into segments as needed )
-void Robot::append_arc(Gcode* gcode, float target[], float offset[], float radius, bool is_clockwise ){
+void Robot::append_arc(Gcode *gcode, float target[], float offset[], float radius, bool is_clockwise )
+{
// Scary math
float center_axis0 = this->last_milestone[this->plane_axis_0] + offset[this->plane_axis_0];
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.
- 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; }
+ 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 = hypotf(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.0001F ){ 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);
+ uint16_t segments = floor(gcode->millimeters_of_travel / this->mm_per_arc_segment);
- float theta_per_segment = angular_travel/segments;
- float 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.
This is important when there are successive arc motions.
*/
// Vector rotation matrix values
- float cos_T = 1-0.5F*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];
// Initialize the linear axis
arc_target[this->plane_axis_2] = this->last_milestone[this->plane_axis_2];
- for (i = 1; i<segments; i++) { // Increment (segments-1)
+ for (i = 1; i < segments; i++) { // Increment (segments-1)
if (count < this->arc_correction ) {
- // Apply vector rotation matrix
- r_axisi = r_axis0*sin_T + r_axis1*cos_T;
- r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
- r_axis1 = r_axisi;
- count++;
+ // Apply vector rotation matrix
+ r_axisi = r_axis0 * sin_T + r_axis1 * cos_T;
+ r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T;
+ r_axis1 = r_axisi;
+ count++;
} 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 = 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 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 = 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;
}
// Update arc_target location
}
// Do the math for an arc and add it to the queue
-void Robot::compute_arc(Gcode* gcode, float offset[], float target[]){
+void Robot::compute_arc(Gcode *gcode, float offset[], float target[])
+{
// Find the radius
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;
- if( this->motion_mode == MOTION_MODE_CW_ARC ){ is_clockwise = true; }
+ if( this->motion_mode == MOTION_MODE_CW_ARC ) {
+ is_clockwise = true;
+ }
// Append arc
this->append_arc(gcode, target, offset, radius, is_clockwise );
}
-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);}}
+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);
+ }
+ }
}
-void Robot::select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2){
+void Robot::select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
+{
this->plane_axis_0 = axis_0;
this->plane_axis_1 = axis_1;
this->plane_axis_2 = axis_2;
}
-void Robot::setToolOffset(float offsetx, float offsety, float offsetz) {
- this->toolOffset[0] = offsetx;
- this->toolOffset[1] = offsety;
- this->toolOffset[2] = offsetz;
+void Robot::setToolOffset(float offsetx, float offsety, float offsetz)
+{
+ this->toolOffset[0] = offsetx;
+ this->toolOffset[1] = offsety;
+ this->toolOffset[2] = offsetz;
}
// Temp sensor implementations:
#include "Thermistor.h"
-#include "max31855.h"
+#include "max31855.h"
#include "MRI_Hooks.h"
#define link_to_tool_checksum CHECKSUM("link_to_tool")
TemperatureControl::TemperatureControl(uint16_t name) :
- sensor(nullptr), name_checksum(name), waiting(false), min_temp_violated(false)
+ sensor(nullptr), name_checksum(name), waiting(false), min_temp_violated(false)
{
}
{
delete sensor;
}
-
-void TemperatureControl::on_module_loaded(){
+
+void TemperatureControl::on_module_loaded()
+{
// We start not desiring any temp
this->target_temperature = UNDEFINED;
this->register_for_event(ON_SET_PUBLIC_DATA);
}
-void TemperatureControl::on_main_loop(void* argument){
+void TemperatureControl::on_main_loop(void *argument)
+{
if (this->min_temp_violated) {
THEKERNEL->streams->printf("Error: MINTEMP triggered. Check your temperature sensors!\n");
this->min_temp_violated = false;
}
// Get configuration from the config file
-void TemperatureControl::on_config_reload(void* argument){
+void TemperatureControl::on_config_reload(void *argument)
+{
// General config
this->set_m_code = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, set_m_code_checksum)->by_default(104)->as_number();
// Instantiate correct sensor (TBD: TempSensor factory?)
delete sensor;
sensor = nullptr; // In case we fail to create a new sensor.
- if(sensor_type.compare("thermistor") == 0)
- {
+ if(sensor_type.compare("thermistor") == 0) {
sensor = new Thermistor();
- }
- else if(sensor_type.compare("max31855") == 0)
- {
+ } else if(sensor_type.compare("max31855") == 0) {
sensor = new Max31855();
- }
- else
- {
+ } else {
sensor = new TempSensor(); // A dummy implementation
}
sensor->UpdateConfig(temperature_control_checksum, this->name_checksum);
-
+
this->preset1 = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, preset1_checksum)->by_default(0)->as_number();
this->preset2 = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, preset2_checksum)->by_default(0)->as_number();
this->heater_pin.set(0);
// used to enable bang bang control of heater
- this->use_bangbang= THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, bang_bang_checksum)->by_default(false)->as_bool();
- this->hysteresis= THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, hysteresis_checksum)->by_default(2)->as_number();
+ this->use_bangbang = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, bang_bang_checksum)->by_default(false)->as_bool();
+ this->hysteresis = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, hysteresis_checksum)->by_default(2)->as_number();
set_low_on_debug(heater_pin.port_number, heater_pin.pin);
// reading tick
THEKERNEL->slow_ticker->attach( this->readings_per_second, this, &TemperatureControl::thermistor_read_tick );
- this->PIDdt= 1.0 / this->readings_per_second;
+ this->PIDdt = 1.0 / this->readings_per_second;
// PID
setPIDp( THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, p_factor_checksum)->by_default(10 )->as_number() );
// set to the same as max_pwm by default
this->i_max = THEKERNEL->config->value(temperature_control_checksum, this->name_checksum, i_max_checksum )->by_default(this->heater_pin.max_pwm())->as_number();
this->iTerm = 0.0;
- this->lastInput= -1.0;
+ this->lastInput = -1.0;
this->last_reading = 0.0;
}
-void TemperatureControl::on_gcode_received(void* argument){
- Gcode* gcode = static_cast<Gcode*>(argument);
+void TemperatureControl::on_gcode_received(void *argument)
+{
+ Gcode *gcode = static_cast<Gcode *>(argument);
if (gcode->has_m) {
// Get temperature
this->active = true;
if( this->link_to_tool ) {
- void* returned_data;
+ void *returned_data;
bool ok = THEKERNEL->public_data->get_value( tool_manager_checksum, &returned_data );
if (ok) {
this->active = toolmanager.current_tool_name == this->name_checksum;
}
}
- if( gcode->m == this->get_m_code ){
+ if( gcode->m == this->get_m_code ) {
char buf[32]; // should be big enough for any status
- int n= snprintf(buf, sizeof(buf), "%s:%3.1f /%3.1f @%d ", this->designator.c_str(), this->get_temperature(), ((target_temperature == UNDEFINED)?0.0:target_temperature), this->o);
+ int n = snprintf(buf, sizeof(buf), "%s:%3.1f /%3.1f @%d ", this->designator.c_str(), this->get_temperature(), ((target_temperature == UNDEFINED) ? 0.0 : target_temperature), this->o);
gcode->txt_after_ok.append(buf, n);
gcode->mark_as_taken();
} else if (gcode->m == 301) {
gcode->mark_as_taken();
- if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index))
- {
+ if (gcode->has_letter('S') && (gcode->get_value('S') == this->pool_index)) {
if (gcode->has_letter('P'))
setPIDp( gcode->get_value('P') );
if (gcode->has_letter('I'))
this->i_max = gcode->get_value('X');
}
//gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g Pv:%g Iv:%g Dv:%g O:%d\n", this->designator.c_str(), this->pool_index, this->p_factor, this->i_factor/this->PIDdt, this->d_factor*this->PIDdt, this->i_max, this->p, this->i, this->d, o);
- gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g O:%d\n", this->designator.c_str(), this->pool_index, this->p_factor, this->i_factor/this->PIDdt, this->d_factor*this->PIDdt, this->i_max, o);
+ gcode->stream->printf("%s(S%d): Pf:%g If:%g Df:%g X(I_max):%g O:%d\n", this->designator.c_str(), this->pool_index, this->p_factor, this->i_factor / this->PIDdt, this->d_factor * this->PIDdt, this->i_max, o);
} else if (gcode->m == 303) {
if (gcode->has_letter('E') && (gcode->get_value('E') == this->pool_index)) {
target = gcode->get_value('S');
gcode->stream->printf("Target: %5.1f\n", target);
}
- int ncycles= 8;
+ int ncycles = 8;
if (gcode->has_letter('C')) {
- ncycles= gcode->get_value('C');
+ ncycles = gcode->get_value('C');
}
gcode->stream->printf("Start PID tune, command is %s\n", gcode->command.c_str());
this->pool->PIDtuner->begin(this, target, gcode->stream, ncycles);
}
- } else if (gcode->m == 500 || gcode->m == 503){// M500 saves some volatile settings to config override file, M503 just prints the settings
- gcode->stream->printf(";PID settings:\nM301 S%d P%1.4f I%1.4f D%1.4f\n", this->pool_index, this->p_factor, this->i_factor/this->PIDdt, this->d_factor*this->PIDdt);
+ } else if (gcode->m == 500 || gcode->m == 503) { // M500 saves some volatile settings to config override file, M503 just prints the settings
+ gcode->stream->printf(";PID settings:\nM301 S%d P%1.4f I%1.4f D%1.4f\n", this->pool_index, this->p_factor, this->i_factor / this->PIDdt, this->d_factor * this->PIDdt);
gcode->mark_as_taken();
} else if( ( gcode->m == this->set_m_code || gcode->m == this->set_and_wait_m_code ) && gcode->has_letter('S') && this->active ) {
}
}
-void TemperatureControl::on_gcode_execute(void* argument){
- Gcode* gcode = static_cast<Gcode*>(argument);
- if( gcode->has_m){
+void TemperatureControl::on_gcode_execute(void *argument)
+{
+ Gcode *gcode = static_cast<Gcode *>(argument);
+ if( gcode->has_m) {
if (((gcode->m == this->set_m_code) || (gcode->m == this->set_and_wait_m_code))
- && gcode->has_letter('S') && this->active)
- {
+ && gcode->has_letter('S') && this->active) {
float v = gcode->get_value('S');
- if (v == 0.0)
- {
+ if (v == 0.0) {
this->target_temperature = UNDEFINED;
- this->heater_pin.set((this->o=0));
- }
- else
- {
+ this->heater_pin.set((this->o = 0));
+ } else {
this->set_desired_temperature(v);
- if( gcode->m == this->set_and_wait_m_code)
- {
+ if( gcode->m == this->set_and_wait_m_code) {
THEKERNEL->pauser->take();
this->waiting = true;
}
}
}
-void TemperatureControl::on_get_public_data(void* argument){
- PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
+void TemperatureControl::on_get_public_data(void *argument)
+{
+ PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
if(!pdr->starts_with(temperature_control_checksum)) return;
if(pdr->third_element_is(current_temperature_checksum)) {
// this must be static as it will be accessed long after we have returned
static struct pad_temperature temp_return;
- temp_return.current_temperature= this->get_temperature();
- temp_return.target_temperature= (target_temperature == UNDEFINED) ? 0 : this->target_temperature;
- temp_return.pwm= this->o;
+ temp_return.current_temperature = this->get_temperature();
+ temp_return.target_temperature = (target_temperature == UNDEFINED) ? 0 : this->target_temperature;
+ temp_return.pwm = this->o;
pdr->set_data_ptr(&temp_return);
pdr->set_taken();
}
}
-void TemperatureControl::on_set_public_data(void* argument){
- PublicDataRequest* pdr = static_cast<PublicDataRequest*>(argument);
+void TemperatureControl::on_set_public_data(void *argument)
+{
+ PublicDataRequest *pdr = static_cast<PublicDataRequest *>(argument);
if(!pdr->starts_with(temperature_control_checksum)) return;
if(!pdr->second_element_is(this->name_checksum)) return; // will be bed or hotend
// ok this is targeted at us, so set the temp
- float t= *static_cast<float*>(pdr->get_data_ptr());
+ float t = *static_cast<float *>(pdr->get_data_ptr());
this->set_desired_temperature(t);
pdr->set_taken();
}
heater_pin.set((this->o = 0));
}
-float TemperatureControl::get_temperature(){
+float TemperatureControl::get_temperature()
+{
return last_reading;
}
-uint32_t TemperatureControl::thermistor_read_tick(uint32_t dummy){
+uint32_t TemperatureControl::thermistor_read_tick(uint32_t dummy)
+{
float temperature = sensor->get_temperature();
- if (target_temperature > 0)
- {
- if (isinf(temperature))
- {
+ if (target_temperature > 0) {
+ if (isinf(temperature)) {
this->min_temp_violated = true;
target_temperature = UNDEFINED;
- heater_pin.set((this->o=0));
- }
- else
- {
+ heater_pin.set((this->o = 0));
+ } else {
pid_process(temperature);
- if ((temperature > target_temperature) && waiting)
- {
+ if ((temperature > target_temperature) && waiting) {
THEKERNEL->pauser->release();
waiting = false;
}
}
- }
- else
- {
+ } else {
heater_pin.set((this->o = 0));
}
last_reading = temperature;
if(use_bangbang) {
// bang bang is very simple, if temp is < target - hysteresis turn on full else if temp is > target + hysteresis turn heater off
// good for relays
- if(temperature > (target_temperature+hysteresis) && this->o > 0) {
+ if(temperature > (target_temperature + hysteresis) && this->o > 0) {
heater_pin.set(false);
- this->o= 0; // for display purposes only
+ this->o = 0; // for display purposes only
- }else if(temperature < (target_temperature-hysteresis) && this->o <= 0) {
+ } else if(temperature < (target_temperature - hysteresis) && this->o <= 0) {
if(heater_pin.max_pwm() >= 255) {
// turn on full
this->heater_pin.set(true);
- this->o= 255; // for display purposes only
- }else{
+ this->o = 255; // for display purposes only
+ } else {
// only to whatever max pwm is configured
this->heater_pin.pwm(heater_pin.max_pwm());
- this->o= heater_pin.max_pwm(); // for display purposes only
+ this->o = heater_pin.max_pwm(); // for display purposes only
}
}
return;
if (this->iTerm > this->i_max) this->iTerm = this->i_max;
else if (this->iTerm < 0.0) this->iTerm = 0.0;
- if(this->lastInput < 0.0) this->lastInput= temperature; // set first time
- float d= (temperature - this->lastInput);
+ if(this->lastInput < 0.0) this->lastInput = temperature; // set first time
+ float d = (temperature - this->lastInput);
// calculate the PID output
// TODO does this need to be scaled by max_pwm/256? I think not as p_factor already does that
- this->o = (this->p_factor*error) + this->iTerm - (this->d_factor*d);
+ this->o = (this->p_factor * error) + this->iTerm - (this->d_factor * d);
if (this->o >= heater_pin.max_pwm())
this->o = heater_pin.max_pwm();
this->o = 0;
this->heater_pin.pwm(this->o);
- this->lastInput= temperature;
+ this->lastInput = temperature;
}
-void TemperatureControl::on_second_tick(void* argument)
+void TemperatureControl::on_second_tick(void *argument)
{
if (waiting)
- THEKERNEL->streams->printf("%s:%3.1f /%3.1f @%d\n", designator.c_str(), get_temperature(), ((target_temperature == UNDEFINED)?0.0:target_temperature), o);
+ THEKERNEL->streams->printf("%s:%3.1f /%3.1f @%d\n", designator.c_str(), get_temperature(), ((target_temperature == UNDEFINED) ? 0.0 : target_temperature), o);
}
-void TemperatureControl::setPIDp(float p) {
- this->p_factor= p;
+void TemperatureControl::setPIDp(float p)
+{
+ this->p_factor = p;
}
-void TemperatureControl::setPIDi(float i) {
- this->i_factor= i*this->PIDdt;
+void TemperatureControl::setPIDi(float i)
+{
+ this->i_factor = i * this->PIDdt;
}
-void TemperatureControl::setPIDd(float d) {
- this->d_factor= d/this->PIDdt;
+void TemperatureControl::setPIDd(float d)
+{
+ this->d_factor = d / this->PIDdt;
}
HCoop / clinton / Smoothieware.git / commitdiff