src/modules/robot/arm_solutions/CoreXZSolution.cpp

   1 #include "CoreXZSolution.h"
   2 #include "ActuatorCoordinates.h"
   3 #include "ConfigValue.h"
   4 #include "checksumm.h"
   5
   6 #define x_reduction_checksum         CHECKSUM("x_reduction")
   7 #define z_reduction_checksum         CHECKSUM("z_reduction")
   8
   9 CoreXZSolution::CoreXZSolution(Config* config)
  10 {
  11     x_reduction = config->value(x_reduction_checksum)->by_default(1.0f)->as_number();
  12     z_reduction = config->value(z_reduction_checksum)->by_default(3.0f)->as_number();
  13 }
  14
  15 void CoreXZSolution::cartesian_to_actuator(const float cartesian_mm[], ActuatorCoordinates &actuator_mm ){
  16     actuator_mm[ALPHA_STEPPER] = (this->x_reduction * cartesian_mm[X_AXIS]) + (this->z_reduction * cartesian_mm[Z_AXIS]);
  17     actuator_mm[BETA_STEPPER ] = (this->x_reduction * cartesian_mm[X_AXIS]) - (this->z_reduction * cartesian_mm[Z_AXIS]);
  18     actuator_mm[GAMMA_STEPPER] = cartesian_mm[Y_AXIS];
  19 }
  20
  21 void CoreXZSolution::actuator_to_cartesian(const ActuatorCoordinates &actuator_mm, float cartesian_mm[] ){
  22     cartesian_mm[X_AXIS] = (0.5F/this->x_reduction) * (actuator_mm[ALPHA_STEPPER] + actuator_mm[BETA_STEPPER]);
  23     cartesian_mm[Z_AXIS] = (0.5F/this->z_reduction) * (actuator_mm[ALPHA_STEPPER] - actuator_mm[BETA_STEPPER]);
  24     cartesian_mm[Y_AXIS] = actuator_mm[GAMMA_STEPPER];
  25 }