adds support for a cartesian system that is rotated from the mechanical system on the z axis #115

Merged
merged 3 commits into from Feb 15, 2013
@@ -19,6 +19,7 @@ using std::string;
#include "../communication/utils/Gcode.h"
#include "arm_solutions/BaseSolution.h"
#include "arm_solutions/CartesianSolution.h"
+#include "arm_solutions/RotatableCartesianSolution.h"
#include "arm_solutions/RostockSolution.h"
Robot::Robot(){
@@ -59,6 +60,9 @@ void Robot::on_config_reload(void* argument){
// place holder for now
this->arm_solution = new RostockSolution(this->kernel->config);
+ }else if(solution_checksum == rotatable_cartesian_checksum) {
+ this->arm_solution = new RotatableCartesianSolution(this->kernel->config);
+
}else if(solution_checksum == cartesian_checksum) {
this->arm_solution = new CartesianSolution(this->kernel->config);
@@ -29,6 +29,7 @@ using std::string;
#define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
#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")
@@ -0,0 +1,53 @@
+#include "RotatableCartesianSolution.h"
+#include <math.h>
+
+#define PIOVER180 0.01745329251994329576923690768489
+
+RotatableCartesianSolution::RotatableCartesianSolution(Config* passed_config) : config(passed_config){
+ this->alpha_steps_per_mm = this->config->value(alpha_steps_per_mm_checksum)->as_number();
+ this->beta_steps_per_mm = this->config->value( beta_steps_per_mm_checksum)->as_number();
+ this->gamma_steps_per_mm = this->config->value(gamma_steps_per_mm_checksum)->as_number();
+
+ double alpha_angle = PIOVER180 * this->config->value(alpha_angle_checksum)->by_default(0.0)->as_number();
+ this->sin_alpha = sin(alpha_angle);
+ this->cos_alpha = cos(alpha_angle);
+}
+
+void RotatableCartesianSolution::millimeters_to_steps( double millimeters[], int steps[] ){
+ double rotated[3];
+ rotate( millimeters, rotated, this->sin_alpha, this->cos_alpha );
+
+ steps[ALPHA_STEPPER] = lround( rotated[X_AXIS] * this->alpha_steps_per_mm );
+ steps[BETA_STEPPER ] = lround( rotated[Y_AXIS] * this->beta_steps_per_mm );
+ steps[GAMMA_STEPPER] = lround( rotated[Z_AXIS] * this->gamma_steps_per_mm );
+}
+
+void RotatableCartesianSolution::steps_to_millimeters( int steps[], double millimeters[] ){
+ double rotated[3];
+
+ rotated[ALPHA_STEPPER] = steps[X_AXIS] / this->alpha_steps_per_mm;
+ rotated[BETA_STEPPER ] = steps[Y_AXIS] / this->beta_steps_per_mm;
+ rotated[GAMMA_STEPPER] = steps[Z_AXIS] / this->gamma_steps_per_mm;
+
+ rotate( rotated, millimeters, - this->sin_alpha, this->cos_alpha );
+}
+
+void RotatableCartesianSolution::rotate(double in[], double out[], double sin, double cos ){
+ out[X_AXIS] = cos * in[X_AXIS] - sin * in[Y_AXIS];
+ out[Y_AXIS] = sin * in[X_AXIS] + cos * in[Y_AXIS];
+ out[Z_AXIS] = in[Z_AXIS];
+}
+
+void RotatableCartesianSolution::set_steps_per_millimeter( double steps[] )
+{
+ this->alpha_steps_per_mm = steps[0];
+ this->beta_steps_per_mm = steps[1];
+ this->gamma_steps_per_mm = steps[2];
+}
+
+void RotatableCartesianSolution::get_steps_per_millimeter( double steps[] )
+{
+ steps[0] = this->alpha_steps_per_mm;
+ steps[1] = this->beta_steps_per_mm;
+ steps[2] = this->gamma_steps_per_mm;
+}
@@ -0,0 +1,38 @@
+#ifndef ROTATABLECARTESIANSOLUTION_H
+#define ROTATABLECARTESIANSOLUTION_H
+#include "libs/Module.h"
+#include "libs/Kernel.h"
+#include "BaseSolution.h"
+#include "libs/nuts_bolts.h"
+
+#include "libs/Config.h"
+
+#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_angle_checksum CHECKSUM("alpha_angle")
+
+class RotatableCartesianSolution : public BaseSolution {
+ public:
+ RotatableCartesianSolution(Config* passed_config);
+ void millimeters_to_steps( double millimeters[], int steps[] );
+ void steps_to_millimeters( int steps[], double millimeters[] );
+
+ void set_steps_per_millimeter( double steps[] );
+ void get_steps_per_millimeter( double steps[] );
+
+ void rotate( double in[], double out[], double sin, double cos );
+
+ Config* config;
+ double alpha_steps_per_mm;
+ double beta_steps_per_mm;
+ double gamma_steps_per_mm;
+
+ double sin_alpha;
+ double cos_alpha;
+};
+
+
+#endif // ROTATABLECARTESIANSOLUTION_H
+