Merge pull request #219 from wolfmanjm/fix/pid2

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

#include "CartesianSolution.h"
#include <math.h>

CartesianSolution::CartesianSolution(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 CartesianSolution::millimeters_to_steps( double millimeters[], int steps[] ){
    steps[ALPHA_STEPPER] = lround( millimeters[X_AXIS] * this->alpha_steps_per_mm );
    steps[BETA_STEPPER ] = lround( millimeters[Y_AXIS] * this->beta_steps_per_mm );
    steps[GAMMA_STEPPER] = lround( millimeters[Z_AXIS] * this->gamma_steps_per_mm );
}

void CartesianSolution::steps_to_millimeters( int steps[], double millimeters[] ){
    millimeters[ALPHA_STEPPER] = steps[X_AXIS] / this->alpha_steps_per_mm;
    millimeters[BETA_STEPPER ] = steps[Y_AXIS] / this->beta_steps_per_mm;
    millimeters[GAMMA_STEPPER] = steps[Z_AXIS] / this->gamma_steps_per_mm;
}

void CartesianSolution::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 CartesianSolution::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;
}