[clinton/Smoothieware.git] / src / modules / tools / scaracal / SCARAcal.cpp

/*
      This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
      Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
      Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
      You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
*/

#include "SCARAcal.h"

#include "Kernel.h"
#include "BaseSolution.h"
#include "Config.h"
#include "Robot.h"
#include "StepperMotor.h"
#include "StreamOutputPool.h"
#include "Gcode.h"
#include "Conveyor.h"
#include "Stepper.h"
#include "checksumm.h"
#include "ConfigValue.h"
#include "SerialMessage.h"
#include "EndstopsPublicAccess.h"
#include "PublicData.h"
#include <algorithm>
#include <map>

#define scaracal_checksum CHECKSUM("scaracal")
#define enable_checksum CHECKSUM("enable")
#define slow_feedrate_checksum CHECKSUM("slow_feedrate")
#define z_move_checksum CHECKSUM("z_move")

#define X_AXIS 0
#define Y_AXIS 1
#define Z_AXIS 2

#define STEPPER THEKERNEL->robot->actuators
#define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm())

void SCARAcal::on_module_loaded()
{
    // if the module is disabled -> do nothing
    if(!THEKERNEL->config->value( scaracal_checksum, enable_checksum )->by_default(false)->as_bool()) {
        // as this module is not needed free up the resource
        delete this;
        return;
    }

    // load settings
    this->on_config_reload(this);
    // register event-handlers
    register_for_event(ON_GCODE_RECEIVED);
}

void SCARAcal::on_config_reload(void *argument)
{
    this->slow_rate = THEKERNEL->config->value( scaracal_checksum, slow_feedrate_checksum )->by_default(5)->as_number(); // feedrate in mm/sec
    this->z_move = THEKERNEL->config->value( scaracal_checksum, z_move_checksum )->by_default(0)->as_number(); // Optional movement of Z relative to the home position.
                                                                                                  // positive values increase distance between nozzle and bed.
                                                                                                  // negative will decrease.  Useful when level code active to prevent collision

}


// issue home command
void SCARAcal::home()
{
    Gcode gc("G28", &(StreamOutput::NullStream));
    THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
}

bool SCARAcal::get_trim(float& x, float& y, float& z)
{
    void *returned_data;
    bool ok = PublicData::get_value( endstops_checksum, trim_checksum, &returned_data );

    if (ok) {
        float *trim = static_cast<float *>(returned_data);
        x= trim[0];
        y= trim[1];
        z= trim[2];
        return true;
    }
    return false;
}

bool SCARAcal::set_trim(float x, float y, float z, StreamOutput *stream)
{
    float t[3]{x, y, z};
    bool ok= PublicData::set_value( endstops_checksum, trim_checksum, t);

    if (ok) {
        stream->printf("set trim to X:%f Y:%f Z:%f\n", x, y, z);
    } else {
        stream->printf("unable to set trim, is endstops enabled?\n");
    }

    return ok;
}

bool SCARAcal::get_home_offset(float& x, float& y, float& z)
{
    void *returned_data;
    bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &returned_data );

    if (ok) {
        float *home_offset = static_cast<float *>(returned_data);
        x= home_offset[0];
        y= home_offset[1];
        z= home_offset[2];
        return true;
    }
    return false;
}

bool SCARAcal::set_home_offset(float x, float y, float z, StreamOutput *stream)
{
    char cmd[64];

    // Assemble Gcode to add onto the queue
    snprintf(cmd, sizeof(cmd), "M206 X%1.3f Y%1.3f Z%1.3f", x, y, z); // Send saved Z homing offset

    Gcode gc(cmd, &(StreamOutput::NullStream));
    THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);

    stream->printf("Set home_offset to X:%f Y:%f Z:%f\n", x, y, z);

    return true;//ok;
}

bool SCARAcal::translate_trim(StreamOutput *stream)
{
    float S_trim[3],
          home_off[3],
          actuator[3];;

    this->get_home_offset(home_off[0], home_off[1], home_off[2]);               // get home offsets
    this->get_trim(S_trim[0], S_trim[1], S_trim[2]);	                          // get current trim

    THEKERNEL->robot->arm_solution->cartesian_to_actuator( home_off, actuator ); // convert current home offset to actuator angles

    // Subtract trim values from actuators to determine the real home offset actuator position for X and Y.

    actuator[0] -= S_trim[0];
    actuator[1] -= S_trim[1];

    // Clear X and Y trims internally
    S_trim[0] = 0.0F;
    S_trim[1] = 0.0F;

    // convert back to get the real cartesian home offsets

    THEKERNEL->robot->arm_solution->actuator_to_cartesian( actuator, home_off );

    this->set_home_offset(home_off[0], home_off[1], home_off[2],stream);               // get home offsets
                 // Set the correct home offsets;

    this->set_trim(S_trim[0], S_trim[1], S_trim[2], stream);    // Now Clear relevant trims

    return true;
}

void SCARAcal::SCARA_ang_move(float theta, float psi, float z, float feedrate)
{
    char cmd[64];
    float actuator[3],
          cartesian[3];

    // Assign the actuator angles from input
    actuator[0] = theta;
    actuator[1] = psi;
    actuator[2] = z;

    // Calculate the physical position relating to the arm angles
    THEKERNEL->robot->arm_solution->actuator_to_cartesian( actuator, cartesian );

    // Assemble Gcode to add onto the queue
    snprintf(cmd, sizeof(cmd), "G0 X%1.3f Y%1.3f Z%1.3f F%1.1f", cartesian[0], cartesian[1], cartesian[2], feedrate * 60); // use specified feedrate (mm/sec)

    //THEKERNEL->streams->printf("DEBUG: move: %s\n", cmd);

    Gcode gc(cmd, &(StreamOutput::NullStream));
    THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
}

//A GCode has been received
//See if the current Gcode line has some orders for us
void SCARAcal::on_gcode_received(void *argument)
{
    Gcode *gcode = static_cast<Gcode *>(argument);

    if( gcode->has_m) {
        switch( gcode->m ) {

            case 114: {    // Extra stuff for Morgan calibration
                char buf[32];
                float cartesian[3],
                      actuators[3];

                THEKERNEL->robot->get_axis_position(cartesian);    // get actual position from robot
                THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators );      // translate to get actuator position

                int n = snprintf(buf, sizeof(buf), "  A: Th:%1.3f Ps:%1.3f",
                                 actuators[0],
                                 actuators[1]);    // display actuator angles Theta and Psi.
                gcode->txt_after_ok.append(buf, n);
            }
            break;

            case 360: {
                float target[2] = {0.0F, 120.0F},
                      cartesian[3],
                      S_trim[3];

                this->get_trim(S_trim[0], S_trim[1], S_trim[2]);	// get current trim to conserve other calbration values

                if(gcode->has_letter('P')) {
                    // Program the current position as target
                    float actuators[3],
                          S_delta[2],
                          S_trim[3];

                    THEKERNEL->robot->get_axis_position(cartesian);    // get actual position from robot
                    THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators );      // translate to get actuator position

                    S_delta[0] = actuators[0] - target[0];

                    set_trim(S_delta[0], S_trim[1], S_trim[2], gcode->stream);
                } else {
                    set_trim(0, S_trim[1], S_trim[2], gcode->stream);               // reset trim for calibration move
                    this->home();                                                   // home
                    THEKERNEL->robot->get_axis_position(cartesian);    // get actual position from robot
                    SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target
                }
            }
            break;

            case 361: {
                float target[2] = {90.0F, 130.0F},
                      cartesian[3];

                if(gcode->has_letter('P')) {
                    // Program the current position as target
                    float actuators[3];

                    THEKERNEL->robot->get_axis_position(cartesian);                                // get actual position from robot
                    THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position

                    STEPPER[0]->change_steps_per_mm(actuators[0] / target[0] * STEPPER[0]->get_steps_per_mm()); // Find angle difference
                    STEPPER[1]->change_steps_per_mm(STEPPER[0]->get_steps_per_mm());  // and change steps_per_mm to ensure correct steps per *angle*
                } else {
                    this->home();                                                   // home - This time leave trims as adjusted.
                    THEKERNEL->robot->get_axis_position(cartesian);    // get actual position from robot
                    SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target
                }

            }
            break;

            case 364: {
                float target[2] = {45.0F, 135.0F},
                      cartesian[3],
                      S_trim[3];

                this->get_trim(S_trim[0], S_trim[1], S_trim[2]);	// get current trim to conserve other calbration values

                if(gcode->has_letter('P')) {
                    // Program the current position as target
                    float actuators[3],
                          S_delta[2];

                    THEKERNEL->robot->get_axis_position(cartesian);                                     // get actual position from robot
                    THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators );      // translate it to get actual actuator angles

                    S_delta[1] = ( actuators[1] - actuators[0]) - ( target[1] - target[0] );            // Find difference in angle - not actuator difference, and
                    set_trim(S_trim[0], S_delta[1], S_trim[2], gcode->stream);                                  // set trim to reflect the difference
                } else {
                    set_trim(S_trim[0], 0, S_trim[2], gcode->stream);                                           // reset trim for calibration move
                    this->home();                                                                       // home
                    THEKERNEL->robot->get_axis_position(cartesian);    // get actual position from robot
                    SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F);                     // move to target
                }
            }
            break;

            case 366:                                       // Translate trims to the actual endstop offsets for SCARA
                this->translate_trim(gcode->stream);
                break;

        }
    }
}
Commit	Line	Data
ed866a4b QH	1	/*
	2	This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
	3	Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
	4	Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
	5	You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
	6	*/
	7
	8	#include "SCARAcal.h"
	9
	10	#include "Kernel.h"
	11	#include "BaseSolution.h"
	12	#include "Config.h"
	13	#include "Robot.h"
	14	#include "StepperMotor.h"
	15	#include "StreamOutputPool.h"
	16	#include "Gcode.h"
	17	#include "Conveyor.h"
	18	#include "Stepper.h"
	19	#include "checksumm.h"
	20	#include "ConfigValue.h"
	21	#include "SerialMessage.h"
	22	#include "EndstopsPublicAccess.h"
	23	#include "PublicData.h"
983e6549 QH	24	#include <algorithm>
983e6549 QH	25	#include <map>
ed866a4b	26
33bacc39 QH	27	#define scaracal_checksum CHECKSUM("scaracal")
	28	#define enable_checksum CHECKSUM("enable")
	29	#define slow_feedrate_checksum CHECKSUM("slow_feedrate")
ad274785	30	#define z_move_checksum CHECKSUM("z_move")
ed866a4b QH	31
	32	#define X_AXIS 0
	33	#define Y_AXIS 1
	34	#define Z_AXIS 2
	35
	36	#define STEPPER THEKERNEL->robot->actuators
	37	#define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm())
ed866a4b QH	38
	39	void SCARAcal::on_module_loaded()
	40	{
	41	// if the module is disabled -> do nothing
33bacc39	42	if(!THEKERNEL->config->value( scaracal_checksum, enable_checksum )->by_default(false)->as_bool()) {
ed866a4b QH	43	// as this module is not needed free up the resource
	44	delete this;
	45	return;
	46	}
	47
	48	// load settings
	49	this->on_config_reload(this);
	50	// register event-handlers
	51	register_for_event(ON_GCODE_RECEIVED);
ed866a4b QH	52	}
	53
	54	void SCARAcal::on_config_reload(void *argument)
	55	{
33bacc39	56	this->slow_rate = THEKERNEL->config->value( scaracal_checksum, slow_feedrate_checksum )->by_default(5)->as_number(); // feedrate in mm/sec
ad274785 QH	57	this->z_move = THEKERNEL->config->value( scaracal_checksum, z_move_checksum )->by_default(0)->as_number(); // Optional movement of Z relative to the home position.
	58	// positive values increase distance between nozzle and bed.
	59	// negative will decrease. Useful when level code active to prevent collision
ed866a4b QH	60
	61	}
	62
ed866a4b QH	63
	64	// issue home command
	65	void SCARAcal::home()
	66	{
	67	Gcode gc("G28", &(StreamOutput::NullStream));
	68	THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
	69	}
	70
157d2308 QH	71	bool SCARAcal::get_trim(float& x, float& y, float& z)
	72	{
	73	void *returned_data;
	74	bool ok = PublicData::get_value( endstops_checksum, trim_checksum, &returned_data );
	75
	76	if (ok) {
	77	float trim = static_cast<float >(returned_data);
	78	x= trim[0];
	79	y= trim[1];
	80	z= trim[2];
	81	return true;
	82	}
	83	return false;
	84	}
	85
ed866a4b QH	86	bool SCARAcal::set_trim(float x, float y, float z, StreamOutput *stream)
	87	{
	88	float t[3]{x, y, z};
	89	bool ok= PublicData::set_value( endstops_checksum, trim_checksum, t);
	90
	91	if (ok) {
	92	stream->printf("set trim to X:%f Y:%f Z:%f\n", x, y, z);
	93	} else {
	94	stream->printf("unable to set trim, is endstops enabled?\n");
	95	}
	96
	97	return ok;
	98	}
	99
157d2308	100	bool SCARAcal::get_home_offset(float& x, float& y, float& z)
ed866a4b QH	101	{
ed866a4b QH	102	void *returned_data;
157d2308	103	bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &returned_data );
ed866a4b QH	104
ed866a4b QH	105	if (ok) {
157d2308 QH	106	float home_offset = static_cast<float >(returned_data);
	107	x= home_offset[0];
	108	y= home_offset[1];
	109	z= home_offset[2];
ed866a4b QH	110	return true;
	111	}
	112	return false;
	113	}
	114
157d2308 QH	115	bool SCARAcal::set_home_offset(float x, float y, float z, StreamOutput *stream)
	116	{
	117	char cmd[64];
	118
	119	// Assemble Gcode to add onto the queue
	120	snprintf(cmd, sizeof(cmd), "M206 X%1.3f Y%1.3f Z%1.3f", x, y, z); // Send saved Z homing offset
	121
	122	Gcode gc(cmd, &(StreamOutput::NullStream));
	123	THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
	124
983e6549	125	stream->printf("Set home_offset to X:%f Y:%f Z:%f\n", x, y, z);
157d2308 QH	126
	127	return true;//ok;
	128	}
	129
	130	bool SCARAcal::translate_trim(StreamOutput *stream)
	131	{
	132	float S_trim[3],
	133	home_off[3],
	134	actuator[3];;
	135
	136	this->get_home_offset(home_off[0], home_off[1], home_off[2]); // get home offsets
	137	this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim
	138
	139	THEKERNEL->robot->arm_solution->cartesian_to_actuator( home_off, actuator ); // convert current home offset to actuator angles
	140
	141	// Subtract trim values from actuators to determine the real home offset actuator position for X and Y.
	142
	143	actuator[0] -= S_trim[0];
	144	actuator[1] -= S_trim[1];
	145
	146	// Clear X and Y trims internally
	147	S_trim[0] = 0.0F;
	148	S_trim[1] = 0.0F;
	149
	150	// convert back to get the real cartesian home offsets
	151
	152	THEKERNEL->robot->arm_solution->actuator_to_cartesian( actuator, home_off );
	153
	154	this->set_home_offset(home_off[0], home_off[1], home_off[2],stream); // get home offsets
	155	// Set the correct home offsets;
	156
	157	this->set_trim(S_trim[0], S_trim[1], S_trim[2], stream); // Now Clear relevant trims
	158
	159	return true;
	160	}
	161
ed866a4b QH	162	void SCARAcal::SCARA_ang_move(float theta, float psi, float z, float feedrate)
ed866a4b QH	163	{
ed866a4b	164	char cmd[64];
ed866a4b QH	165	float actuator[3],
	166	cartesian[3];
	167
	168	// Assign the actuator angles from input
	169	actuator[0] = theta;
	170	actuator[1] = psi;
	171	actuator[2] = z;
	172
	173	// Calculate the physical position relating to the arm angles
	174	THEKERNEL->robot->arm_solution->actuator_to_cartesian( actuator, cartesian );
	175
	176	// Assemble Gcode to add onto the queue
c2663be2	177	snprintf(cmd, sizeof(cmd), "G0 X%1.3f Y%1.3f Z%1.3f F%1.1f", cartesian[0], cartesian[1], cartesian[2], feedrate * 60); // use specified feedrate (mm/sec)
ed866a4b	178
33bacc39	179	//THEKERNEL->streams->printf("DEBUG: move: %s\n", cmd);
ed866a4b	180
e0c008fe QH	181	Gcode gc(cmd, &(StreamOutput::NullStream));
e0c008fe QH	182	THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
ed866a4b QH	183	}
	184
	185	//A GCode has been received
	186	//See if the current Gcode line has some orders for us
	187	void SCARAcal::on_gcode_received(void *argument)
	188	{
	189	Gcode gcode = static_cast<Gcode >(argument);
	190
	191	if( gcode->has_m) {
	192	switch( gcode->m ) {
	193
	194	case 114: { // Extra stuff for Morgan calibration
	195	char buf[32];
	196	float cartesian[3],
	197	actuators[3];
	198
	199	THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
	200	THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
	201
e0c008fe	202	int n = snprintf(buf, sizeof(buf), " A: Th:%1.3f Ps:%1.3f",
ed866a4b	203	actuators[0],
33bacc39	204	actuators[1]); // display actuator angles Theta and Psi.
ed866a4b	205	gcode->txt_after_ok.append(buf, n);
ed866a4b	206	}
6e92ab91	207	break;
157d2308	208
ed866a4b QH	209	case 360: {
ed866a4b QH	210	float target[2] = {0.0F, 120.0F},
ad274785	211	cartesian[3],
ed866a4b QH	212	S_trim[3];
	213
	214	this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim to conserve other calbration values
	215
	216	if(gcode->has_letter('P')) {
	217	// Program the current position as target
ad274785	218	float actuators[3],
ed866a4b QH	219	S_delta[2],
	220	S_trim[3];
	221
	222	THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
	223	THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
	224
	225	S_delta[0] = actuators[0] - target[0];
	226
ad274785	227	set_trim(S_delta[0], S_trim[1], S_trim[2], gcode->stream);
ed866a4b	228	} else {
ad274785	229	set_trim(0, S_trim[1], S_trim[2], gcode->stream); // reset trim for calibration move
ed866a4b	230	this->home(); // home
ad274785 QH	231	THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
ad274785 QH	232	SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target
ed866a4b	233	}
ed866a4b	234	}
6e92ab91 JM	235	break;
6e92ab91 JM	236
ed866a4b	237	case 361: {
ad274785 QH	238	float target[2] = {90.0F, 130.0F},
	239	cartesian[3];
	240
ed866a4b QH	241	if(gcode->has_letter('P')) {
ed866a4b QH	242	// Program the current position as target
ad274785	243	float actuators[3];
ed866a4b QH	244
	245	THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
	246	THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
	247
33bacc39	248	STEPPER[0]->change_steps_per_mm(actuators[0] / target[0] * STEPPER[0]->get_steps_per_mm()); // Find angle difference
157d2308	249	STEPPER[1]->change_steps_per_mm(STEPPER[0]->get_steps_per_mm()); // and change steps_per_mm to ensure correct steps per angle
ed866a4b QH	250	} else {
ed866a4b QH	251	this->home(); // home - This time leave trims as adjusted.
ad274785 QH	252	THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
ad274785 QH	253	SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target
ed866a4b	254	}
6e92ab91	255
ed866a4b	256	}
6e92ab91 JM	257	break;
	258
	259	case 364: {
ed866a4b	260	float target[2] = {45.0F, 135.0F},
ad274785	261	cartesian[3],
ed866a4b QH	262	S_trim[3];
	263
	264	this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim to conserve other calbration values
	265
	266	if(gcode->has_letter('P')) {
	267	// Program the current position as target
ad274785	268	float actuators[3],
ed866a4b QH	269	S_delta[2];
ed866a4b QH	270
33bacc39 QH	271	THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
33bacc39 QH	272	THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate it to get actual actuator angles
ed866a4b	273
1a15649d	274	S_delta[1] = ( actuators[1] - actuators[0]) - ( target[1] - target[0] ); // Find difference in angle - not actuator difference, and
ad274785	275	set_trim(S_trim[0], S_delta[1], S_trim[2], gcode->stream); // set trim to reflect the difference
ed866a4b	276	} else {
ad274785	277	set_trim(S_trim[0], 0, S_trim[2], gcode->stream); // reset trim for calibration move
1a15649d	278	this->home(); // home
ad274785 QH	279	THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
ad274785 QH	280	SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target
ed866a4b	281	}
ed866a4b	282	}
6e92ab91	283	break;
157d2308	284
6e92ab91	285	case 366: // Translate trims to the actual endstop offsets for SCARA
157d2308	286	this->translate_trim(gcode->stream);
6e92ab91	287	break;
983e6549	288
ed866a4b	289	}
157d2308	290	}
ed866a4b QH	291	}
ed866a4b QH	292