Initial commit of HBotSolution #152

Merged
merged 1 commit into from Mar 25, 2013
@@ -21,6 +21,7 @@ using std::string;
#include "arm_solutions/CartesianSolution.h"
#include "arm_solutions/RotatableCartesianSolution.h"
#include "arm_solutions/RostockSolution.h"
+#include "arm_solutions/HBotSolution.h"
Robot::Robot(){
this->inch_mode = false;
@@ -53,7 +54,10 @@ void Robot::on_config_reload(void* argument){
int solution_checksum = get_checksum(this->kernel->config->value(arm_solution_checksum)->by_default("cartesian")->as_string());
// Note checksums are not const expressions when in debug mode, so don't use switch
- if(solution_checksum == rostock_checksum) {
+ if(solution_checksum == hbot_checksum) {
+ this->arm_solution = new HBotSolution(this->kernel->config);
+
+ }else if(solution_checksum == rostock_checksum) {
this->arm_solution = new RostockSolution(this->kernel->config);
}else if(solution_checksum == delta_checksum) {
@@ -33,6 +33,7 @@ using std::string;
#define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
#define rostock_checksum CHECKSUM("rostock")
#define delta_checksum CHECKSUM("delta")
+#define hbot_checksum CHECKSUM("hbot")
#define NEXT_ACTION_DEFAULT 0
#define NEXT_ACTION_DWELL 1
@@ -0,0 +1,38 @@
+#include "HBotSolution.h"
+#include <math.h>
+
+HBotSolution::HBotSolution(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();
+}
+
+void HBotSolution::millimeters_to_steps( double millimeters[], int steps[] ){
+ int delta_x = lround( millimeters[X_AXIS] * this->alpha_steps_per_mm );
+ int delta_y = lround( millimeters[Y_AXIS] * this->beta_steps_per_mm );
+ steps[ALPHA_STEPPER] = delta_x + delta_y;
+ steps[BETA_STEPPER ] = delta_x - delta_y;
+ steps[GAMMA_STEPPER] = lround( millimeters[Z_AXIS] * this->gamma_steps_per_mm );
+}
+
+void HBotSolution::steps_to_millimeters( int steps[], double millimeters[] ){
+ int delta_alpha = steps[X_AXIS] / this->alpha_steps_per_mm;
+ int delta_beta = steps[Y_AXIS] / this->beta_steps_per_mm;
+ millimeters[ALPHA_STEPPER] = 0.5*(delta_alpha + delta_beta);
+ millimeters[BETA_STEPPER ] = 0.5*(delta_alpha - delta_beta);
+ millimeters[GAMMA_STEPPER] = steps[Z_AXIS] / this->gamma_steps_per_mm;
+}
+
+void HBotSolution::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 HBotSolution::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,35 @@
+#ifndef HBOTSOLUTION_H
+#define HBOTSOLUTION_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")
+
+class HBotSolution : public BaseSolution {
+ public:
+ HBotSolution(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[] );
+
+ Config* config;
+ double alpha_steps_per_mm;
+ double beta_steps_per_mm;
+ double gamma_steps_per_mm;
+};
+
+
+
+
+
+
+#endif // HBOTSOLUTION_H
+