src/modules/utils/currentcontrol/CurrentControl.cpp

   1 #include "CurrentControl.h"
   2 #include "libs/Kernel.h"
   3 #include "libs/nuts_bolts.h"
   4 #include "libs/utils.h"
   5 #include "ConfigValue.h"
   6 #include "libs/StreamOutput.h"
   7
   8 #include "Gcode.h"
   9 #include "Config.h"
  10 #include "checksumm.h"
  11 #include "DigipotBase.h"
  12
  13 // add new digipot chips here
  14 #include "mcp4451.h"
  15 #include "ad5206.h"
  16
  17 #include <string>
  18 using namespace std;
  19
  20 #define alpha_current_checksum                  CHECKSUM("alpha_current")
  21 #define beta_current_checksum                   CHECKSUM("beta_current")
  22 #define gamma_current_checksum                  CHECKSUM("gamma_current")
  23 #define delta_current_checksum                  CHECKSUM("delta_current")
  24 #define epsilon_current_checksum                CHECKSUM("epsilon_current")
  25 #define zeta_current_checksum                   CHECKSUM("zeta_current")
  26 #define eta_current_checksum                    CHECKSUM("eta_current")
  27 #define theta_current_checksum                  CHECKSUM("theta_current")
  28 #define currentcontrol_module_enable_checksum   CHECKSUM("currentcontrol_module_enable")
  29 #define digipotchip_checksum                    CHECKSUM("digipotchip")
  30 #define digipot_max_current                     CHECKSUM("digipot_max_current")
  31 #define digipot_factor                          CHECKSUM("digipot_factor")
  32
  33 #define mcp4451_checksum                        CHECKSUM("mcp4451")
  34 #define ad5206_checksum                         CHECKSUM("ad5206")
  35
  36 CurrentControl::CurrentControl()
  37 {
  38     digipot = NULL;
  39 }
  40
  41 void CurrentControl::on_module_loaded()
  42 {
  43     if( !THEKERNEL->config->value( currentcontrol_module_enable_checksum )->by_default(false)->as_bool() ) {
  44         // as this module is not needed free up the resource
  45         delete this;
  46         return;
  47     }
  48
  49     // allocate digipot, if already allocated delete it first
  50     delete digipot;
  51
  52     // see which chip to use
  53     int chip_checksum = get_checksum(THEKERNEL->config->value(digipotchip_checksum)->by_default("mcp4451")->as_string());
  54     if(chip_checksum == mcp4451_checksum) {
  55         digipot = new MCP4451();
  56     } else if(chip_checksum == ad5206_checksum) {
  57         digipot = new AD5206();
  58     } else { // need a default so use smoothie
  59         digipot = new MCP4451();
  60     }
  61
  62     digipot->set_max_current( THEKERNEL->config->value(digipot_max_current )->by_default(2.0f)->as_number());
  63     digipot->set_factor( THEKERNEL->config->value(digipot_factor )->by_default(113.33f)->as_number());
  64
  65     // Get configuration
  66     this->digipot->set_current(0, THEKERNEL->config->value(alpha_current_checksum  )->by_default(0.8f)->as_number());
  67     this->digipot->set_current(1, THEKERNEL->config->value(beta_current_checksum   )->by_default(0.8f)->as_number());
  68     this->digipot->set_current(2, THEKERNEL->config->value(gamma_current_checksum  )->by_default(0.8f)->as_number());
  69     this->digipot->set_current(3, THEKERNEL->config->value(delta_current_checksum  )->by_default(0.8f)->as_number());
  70     this->digipot->set_current(4, THEKERNEL->config->value(epsilon_current_checksum)->by_default(-1)->as_number());
  71     this->digipot->set_current(5, THEKERNEL->config->value(zeta_current_checksum   )->by_default(-1)->as_number());
  72     this->digipot->set_current(6, THEKERNEL->config->value(eta_current_checksum    )->by_default(-1)->as_number());
  73     this->digipot->set_current(7, THEKERNEL->config->value(theta_current_checksum  )->by_default(-1)->as_number());
  74
  75
  76     this->register_for_event(ON_GCODE_RECEIVED);
  77 }
  78
  79
  80 void CurrentControl::on_gcode_received(void *argument)
  81 {
  82     Gcode *gcode = static_cast<Gcode*>(argument);
  83     if (gcode->has_m) {
  84         if (gcode->m == 907) {
  85             if(gcode->has_letter('E')) { // thi smeans it was the old setting, E is no longer allowed
  86                 char alpha[] = { 'X', 'Y', 'Z', 'E', 'A', 'B', 'C' };
  87                 gcode->stream->printf("WARNING: Using E is deprecated, use A and B for channels 3 and 4\n");
  88                 // this is the old format where E => A, A => B, B => C, C => D
  89                 for (int i = 0; i < 7; i++) {
  90                     if (gcode->has_letter(alpha[i])) {
  91                         this->digipot->set_current(i, gcode->get_value(alpha[i]));
  92                     }
  93                 }
  94
  95             }else{
  96                 // new format where XYZ are the first 3 channels and ABCD are the next channels
  97                 for (int i = 0; i < 7; i++) {
  98                     char axis= i < 3 ? 'X'+i : 'A'+i-3;
  99                     if (gcode->has_letter(axis)) {
 100                         float c = gcode->get_value(axis);
 101                         this->digipot->set_current(i, c);
 102                     }
 103                 }
 104             }
 105
 106         } else if(gcode->m == 500 || gcode->m == 503) {
 107             float currents[7];
 108             bool has_setting= false;
 109             for (int i = 0; i < 7; i++) {
 110                 currents[i]= this->digipot->get_current(i);
 111                 if(currents[i] >= 0) has_setting= true;
 112             }
 113             if(!has_setting) return; // don't ouput anything if none are set using this current control
 114
 115             gcode->stream->printf(";Digipot Motor currents:\nM907 ");
 116             for (int i = 0; i < 7; i++) {
 117                 char axis= i < 3 ? 'X'+i : 'A'+i-3;
 118                 if(currents[i] >= 0)
 119                     gcode->stream->printf("%c%1.5f ", axis, currents[i]);
 120             }
 121             gcode->stream->printf("\n");
 122         }
 123     }
 124 }