[clinton/Smoothieware.git] / src / modules / robot / arm_solutions / MorganSCARASolution.cpp

#include "MorganSCARASolution.h"
#include <fastmath.h>
#include "checksumm.h"
#include "ConfigValue.h"
#include "libs/Kernel.h"
//#include "StreamOutputPool.h"
//#include "Gcode.h"
//#include "SerialMessage.h"
//#include "Conveyor.h"
//#include "Robot.h"
//#include "StepperMotor.h"

#include "libs/nuts_bolts.h"

#include "libs/Config.h"

#define arm1_length_checksum          CHECKSUM("arm1_length")
#define arm2_length_checksum          CHECKSUM("arm2_length")
#define morgan_offset_x_checksum      CHECKSUM("morgan_offset_x")
#define morgan_offset_y_checksum      CHECKSUM("morgan_offset_y")
#define axis_scaling_x_checksum       CHECKSUM("axis_scaling_x")
#define axis_scaling_y_checksum       CHECKSUM("axis_scaling_y")
#define morgan_homing_checksum        CHECKSUM("morgan_homing")
#define morgan_undefined_min_checksum CHECKSUM("morgan_undefined_min")
#define morgan_undefined_max_checksum CHECKSUM("morgan_undefined_max")

#define SQ(x) powf(x, 2)
#define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y)

MorganSCARASolution::MorganSCARASolution(Config* config)
{
    // arm1_length is the length of the inner main arm from hinge to hinge
    arm1_length         = config->value(arm1_length_checksum)->by_default(150.0f)->as_number();
    // arm2_length is the length of the inner main arm from hinge to hinge
    arm2_length         = config->value(arm2_length_checksum)->by_default(150.0f)->as_number();
    // morgan_offset_x is the x offset of bed zero position towards the SCARA tower center
    morgan_offset_x     = config->value(morgan_offset_x_checksum)->by_default(100.0f)->as_number();
    // morgan_offset_y is the y offset of bed zero position towards the SCARA tower center
    morgan_offset_y     = config->value(morgan_offset_y_checksum)->by_default(-60.0f)->as_number();
    // morgan_undefined is the ratio at which the SCARA position is undefined.
    // required to prevent the arm moving through singularity points
    // min: head close to tower
    morgan_undefined_min  = config->value(morgan_undefined_min_checksum)->by_default(0.95f)->as_number();
    // max: head on maximum reach
    morgan_undefined_max  = config->value(morgan_undefined_max_checksum)->by_default(0.95f)->as_number();

    init();
}

void MorganSCARASolution::init() {

}

float MorganSCARASolution::to_degrees(float radians) {
    return radians*(180.0F/3.14159265359f);
}

void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] )
{

    float SCARA_pos[2],
          SCARA_C2,
          SCARA_S2,
          SCARA_K1,
          SCARA_K2,
          SCARA_theta,
          SCARA_psi;

    SCARA_pos[X_AXIS] = cartesian_mm[X_AXIS] - this->morgan_offset_x;  //Translate cartesian to tower centric SCARA X Y
    SCARA_pos[Y_AXIS] = cartesian_mm[Y_AXIS] - this->morgan_offset_y;  // morgan_offset not to be confused with home offset. Makes the SCARA math work.

    if (this->arm1_length == this->arm2_length)
        SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-2.0f*SQ(this->arm1_length)) / (2.0f * SQ(this->arm1_length));
    else
        SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-SQ(this->arm1_length)-SQ(this->arm2_length)) / (2.0f * SQ(this->arm1_length));

    // SCARA position is undefined if abs(SCARA_C2) >=1
    // In reality abs(SCARA_C2) >0.95 is problematic.

    if (SCARA_C2 > this->morgan_undefined_max)
        SCARA_C2 = this->morgan_undefined_max;
    else if (SCARA_C2 < -this->morgan_undefined_min)
        SCARA_C2 = -this->morgan_undefined_min;


    SCARA_S2 = sqrtf(1.0f-SQ(SCARA_C2));

    SCARA_K1 = this->arm1_length+this->arm2_length*SCARA_C2;
    SCARA_K2 = this->arm2_length*SCARA_S2;

    SCARA_theta = (atan2f(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2f(SCARA_K1, SCARA_K2))*-1.0f;    // Morgan Thomas turns Theta in oposite direction
    SCARA_psi   = atan2f(SCARA_S2,SCARA_C2);


    actuator_mm[ALPHA_STEPPER] = to_degrees(SCARA_theta);             // Multiply by 180/Pi  -  theta is support arm angle
    actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_theta + SCARA_psi); // Morgan kinematics (dual arm)
    //actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_psi);             // real scara
    actuator_mm[GAMMA_STEPPER] = cartesian_mm[Z_AXIS];                // No inverse kinematics on Z - Position to add bed offset?

}

void MorganSCARASolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ) {
    // Perform forward kinematics, and place results in cartesian_mm[]

    float y1, y2,
           actuator_rad[2];

    actuator_rad[X_AXIS] = actuator_mm[X_AXIS]/(180.0F/3.14159265359f);
    actuator_rad[Y_AXIS] = actuator_mm[Y_AXIS]/(180.0F/3.14159265359f);

    y1 = sinf(actuator_rad[X_AXIS])*this->arm1_length;
    y2 = sinf(actuator_rad[Y_AXIS])*this->arm2_length + y1;

    cartesian_mm[X_AXIS] = cosf(actuator_rad[X_AXIS])*this->arm1_length + cosf(actuator_rad[Y_AXIS])*this->arm2_length + this->morgan_offset_x;
    cartesian_mm[Y_AXIS] = y2 + this->morgan_offset_y;
    cartesian_mm[Z_AXIS] = actuator_mm[Z_AXIS];

    cartesian_mm[0] = ROUND(cartesian_mm[0], 4);
    cartesian_mm[1] = ROUND(cartesian_mm[1], 4);
    cartesian_mm[2] = ROUND(cartesian_mm[2], 4);
}

bool MorganSCARASolution::set_optional(const arm_options_t& options) {

    arm_options_t::const_iterator i;

    i= options.find('T');          // Theta arm1 length
    if(i != options.end()) {
        arm1_length= i->second;

    }
    i= options.find('P');          // Psi arm2 length
    if(i != options.end()) {
        arm2_length= i->second;
    }
    i= options.find('X');          // Home initial position X
    if(i != options.end()) {
        morgan_offset_x= i->second;
    }
    i= options.find('Y');          // Home initial position Y
    if(i != options.end()) {
        morgan_offset_y= i->second;
    }

    init();
    return true;
}

bool MorganSCARASolution::get_optional(arm_options_t& options) {
    options['T']= this->arm1_length;
    options['P']= this->arm2_length;
    options['X']= this->morgan_offset_x;
    options['Y']= this->morgan_offset_y;
    return true;
};
Commit	Line	Data
1217e470 QH	1	#include "MorganSCARASolution.h"
	2	#include <fastmath.h>
	3	#include "checksumm.h"
	4	#include "ConfigValue.h"
	5	#include "libs/Kernel.h"
	6	//#include "StreamOutputPool.h"
	7	//#include "Gcode.h"
	8	//#include "SerialMessage.h"
	9	//#include "Conveyor.h"
	10	//#include "Robot.h"
	11	//#include "StepperMotor.h"
	12
	13	#include "libs/nuts_bolts.h"
	14
	15	#include "libs/Config.h"
	16
2490d558 QH	17	#define arm1_length_checksum CHECKSUM("arm1_length")
	18	#define arm2_length_checksum CHECKSUM("arm2_length")
	19	#define morgan_offset_x_checksum CHECKSUM("morgan_offset_x")
	20	#define morgan_offset_y_checksum CHECKSUM("morgan_offset_y")
	21	#define axis_scaling_x_checksum CHECKSUM("axis_scaling_x")
	22	#define axis_scaling_y_checksum CHECKSUM("axis_scaling_y")
	23	#define morgan_homing_checksum CHECKSUM("morgan_homing")
	24	#define morgan_undefined_min_checksum CHECKSUM("morgan_undefined_min")
	25	#define morgan_undefined_max_checksum CHECKSUM("morgan_undefined_max")
1217e470 QH	26
	27	#define SQ(x) powf(x, 2)
	28	#define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y)
	29
	30	MorganSCARASolution::MorganSCARASolution(Config* config)
	31	{
	32	// arm1_length is the length of the inner main arm from hinge to hinge
	33	arm1_length = config->value(arm1_length_checksum)->by_default(150.0f)->as_number();
	34	// arm2_length is the length of the inner main arm from hinge to hinge
	35	arm2_length = config->value(arm2_length_checksum)->by_default(150.0f)->as_number();
	36	// morgan_offset_x is the x offset of bed zero position towards the SCARA tower center
	37	morgan_offset_x = config->value(morgan_offset_x_checksum)->by_default(100.0f)->as_number();
	38	// morgan_offset_y is the y offset of bed zero position towards the SCARA tower center
2490d558 QH	39	morgan_offset_y = config->value(morgan_offset_y_checksum)->by_default(-60.0f)->as_number();
	40	// morgan_undefined is the ratio at which the SCARA position is undefined.
	41	// required to prevent the arm moving through singularity points
	42	// min: head close to tower
	43	morgan_undefined_min = config->value(morgan_undefined_min_checksum)->by_default(0.95f)->as_number();
	44	// max: head on maximum reach
	45	morgan_undefined_max = config->value(morgan_undefined_max_checksum)->by_default(0.95f)->as_number();
1217e470 QH	46
	47	init();
	48	}
	49
	50	void MorganSCARASolution::init() {
	51
	52	}
	53
	54	float MorganSCARASolution::to_degrees(float radians) {
	55	return radians*(180.0F/3.14159265359f);
	56	}
	57
	58	void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] )
	59	{
	60
	61	float SCARA_pos[2],
	62	SCARA_C2,
	63	SCARA_S2,
	64	SCARA_K1,
	65	SCARA_K2,
	66	SCARA_theta,
	67	SCARA_psi;
2490d558	68
1217e470 QH	69	SCARA_pos[X_AXIS] = cartesian_mm[X_AXIS] - this->morgan_offset_x; //Translate cartesian to tower centric SCARA X Y
1217e470 QH	70	SCARA_pos[Y_AXIS] = cartesian_mm[Y_AXIS] - this->morgan_offset_y; // morgan_offset not to be confused with home offset. Makes the SCARA math work.
2490d558	71
1217e470 QH	72	if (this->arm1_length == this->arm2_length)
	73	SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-2.0fSQ(this->arm1_length)) / (2.0f SQ(this->arm1_length));
	74	else
900601b5 QH	75	SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-SQ(this->arm1_length)-SQ(this->arm2_length)) / (2.0f * SQ(this->arm1_length));
	76
	77	// SCARA position is undefined if abs(SCARA_C2) >=1
	78	// In reality abs(SCARA_C2) >0.95 is problematic.
	79
2490d558 QH	80	if (SCARA_C2 > this->morgan_undefined_max)
	81	SCARA_C2 = this->morgan_undefined_max;
	82	else if (SCARA_C2 < -this->morgan_undefined_min)
	83	SCARA_C2 = -this->morgan_undefined_min;
	84
900601b5	85
1217e470 QH	86	SCARA_S2 = sqrtf(1.0f-SQ(SCARA_C2));
	87
	88	SCARA_K1 = this->arm1_length+this->arm2_length*SCARA_C2;
	89	SCARA_K2 = this->arm2_length*SCARA_S2;
2490d558	90
1217e470 QH	91	SCARA_theta = (atan2f(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2f(SCARA_K1, SCARA_K2))*-1.0f; // Morgan Thomas turns Theta in oposite direction
1217e470 QH	92	SCARA_psi = atan2f(SCARA_S2,SCARA_C2);
2490d558 QH	93
2490d558 QH	94
1217e470 QH	95	actuator_mm[ALPHA_STEPPER] = to_degrees(SCARA_theta); // Multiply by 180/Pi - theta is support arm angle
	96	actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_theta + SCARA_psi); // Morgan kinematics (dual arm)
	97	//actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_psi); // real scara
	98	actuator_mm[GAMMA_STEPPER] = cartesian_mm[Z_AXIS]; // No inverse kinematics on Z - Position to add bed offset?
	99
	100	}
	101
	102	void MorganSCARASolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ) {
	103	// Perform forward kinematics, and place results in cartesian_mm[]
2490d558	104
1217e470 QH	105	float y1, y2,
	106	actuator_rad[2];
	107
	108	actuator_rad[X_AXIS] = actuator_mm[X_AXIS]/(180.0F/3.14159265359f);
	109	actuator_rad[Y_AXIS] = actuator_mm[Y_AXIS]/(180.0F/3.14159265359f);
	110
	111	y1 = sinf(actuator_rad[X_AXIS])*this->arm1_length;
	112	y2 = sinf(actuator_rad[Y_AXIS])*this->arm2_length + y1;
2490d558	113
1217e470 QH	114	cartesian_mm[X_AXIS] = cosf(actuator_rad[X_AXIS])this->arm1_length + cosf(actuator_rad[Y_AXIS])this->arm2_length + this->morgan_offset_x;
	115	cartesian_mm[Y_AXIS] = y2 + this->morgan_offset_y;
	116	cartesian_mm[Z_AXIS] = actuator_mm[Z_AXIS];
	117
	118	cartesian_mm[0] = ROUND(cartesian_mm[0], 4);
	119	cartesian_mm[1] = ROUND(cartesian_mm[1], 4);
	120	cartesian_mm[2] = ROUND(cartesian_mm[2], 4);
	121	}
	122
	123	bool MorganSCARASolution::set_optional(const arm_options_t& options) {
	124
	125	arm_options_t::const_iterator i;
	126
	127	i= options.find('T'); // Theta arm1 length
	128	if(i != options.end()) {
	129	arm1_length= i->second;
	130
	131	}
	132	i= options.find('P'); // Psi arm2 length
	133	if(i != options.end()) {
	134	arm2_length= i->second;
	135	}
	136	i= options.find('X'); // Home initial position X
	137	if(i != options.end()) {
	138	morgan_offset_x= i->second;
	139	}
	140	i= options.find('Y'); // Home initial position Y
	141	if(i != options.end()) {
	142	morgan_offset_y= i->second;
	143	}
2490d558	144
1217e470 QH	145	init();
	146	return true;
	147	}
	148
	149	bool MorganSCARASolution::get_optional(arm_options_t& options) {
	150	options['T']= this->arm1_length;
	151	options['P']= this->arm2_length;
	152	options['X']= this->morgan_offset_x;
	153	options['Y']= this->morgan_offset_y;
	154	return true;
	155	};