src/modules/tools/rotarydeltacalibration/RotaryDeltaCalibration.cpp

   1 #include "RotaryDeltaCalibration.h"
   2 #include "EndstopsPublicAccess.h"
   3 #include "Kernel.h"
   4 #include "Robot.h"
   5 #include "Config.h"
   6 #include "checksumm.h"
   7 #include "ConfigValue.h"
   8 #include "StreamOutput.h"
   9 #include "PublicDataRequest.h"
  10 #include "PublicData.h"
  11 #include "Gcode.h"
  12 #include "StepperMotor.h"
  13
  14 #define rotarydelta_checksum CHECKSUM("rotary_delta_calibration")
  15 #define enable_checksum CHECKSUM("enable")
  16
  17 void RotaryDeltaCalibration::on_module_loaded()
  18 {
  19     // if the module is disabled -> do nothing
  20     if(!THEKERNEL->config->value( rotarydelta_checksum, enable_checksum )->by_default(false)->as_bool()) {
  21         // as this module is not needed free up the resource
  22         delete this;
  23         return;
  24     }
  25
  26     // register event-handlers
  27     register_for_event(ON_GCODE_RECEIVED);
  28 }
  29
  30 float *RotaryDeltaCalibration::get_homing_offset()
  31 {
  32     float *theta_offset; // points to theta offset in Endstop module
  33     bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &theta_offset );
  34     return ok ? theta_offset : nullptr;
  35 }
  36
  37 void RotaryDeltaCalibration::on_gcode_received(void *argument)
  38 {
  39     Gcode *gcode = static_cast<Gcode *>(argument);
  40
  41     if( gcode->has_m) {
  42         switch( gcode->m ) {
  43             case 206: {
  44                 float *theta_offset= get_homing_offset(); // points to theta offset in Endstop module
  45                 if (theta_offset == nullptr) {
  46                     gcode->stream->printf("error:no endstop module found\n");
  47                     return;
  48                 }
  49
  50                 // set theta offset, set directly in the Endstop module (bad practice really)
  51                 if (gcode->has_letter('A')) theta_offset[0] = gcode->get_value('A');
  52                 if (gcode->has_letter('B')) theta_offset[1] = gcode->get_value('B');
  53                 if (gcode->has_letter('C')) theta_offset[2] = gcode->get_value('C');
  54
  55                 gcode->stream->printf("Theta offset set: A %8.5f B %8.5f C %8.5f\n", theta_offset[0], theta_offset[1], theta_offset[2]);
  56
  57                 break;
  58             }
  59
  60             case 306: {
  61                 // for a rotary delta M306 calibrates the homing angle
  62                 // by doing M306 A-56.17 it will calculate the M206 A value (the theta offset for actuator A) based on the difference
  63                 // between what it thinks is the current angle and what the current angle actually is specified by A (ditto for B and C)
  64
  65                 ActuatorCoordinates current_angle;
  66                 // get the current angle for each actuator, NOTE we only deal with  ABC so if there are more than 3 actuators this will probably go wonky
  67                 for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
  68                     current_angle[i]= THEKERNEL->robot->actuators[i]->get_current_position();
  69                 }
  70
  71                 if (gcode->has_letter('L') && gcode->get_value('L') != 0) {
  72                     // specifying L1 it will take the last probe position (set by G30 or G38.x ) and set the home offset based on that
  73                     // this allows the use of G30 to find the angle tool
  74                     uint8_t ok;
  75                     std::tie(current_angle[0], current_angle[1], current_angle[2], ok) = THEKERNEL->robot->get_last_probe_position();
  76                     if(ok == 0) {
  77                         gcode->stream->printf("error:Nothing set as probe failed or not run\n");
  78                         return;
  79                     }
  80                 }
  81
  82                 float *theta_offset= get_homing_offset(); // points to theta offset in Endstop module
  83                 if (theta_offset == nullptr) {
  84                     gcode->stream->printf("error:no endstop module found\n");
  85                     return;
  86                 }
  87
  88                 int cnt= 0;
  89
  90                 //figure out what home_offset needs to be to correct the homing_position
  91                 if (gcode->has_letter('A')) {
  92                     float a = gcode->get_value('A'); // what actual angle is
  93                     theta_offset[0] -= (current_angle[0] - a);
  94                     current_angle[0]= a;
  95                     cnt++;
  96                 }
  97                 if (gcode->has_letter('B')) {
  98                     float b = gcode->get_value('B');
  99                     theta_offset[1] -= (current_angle[1] - b);
 100                     current_angle[1]= b;
 101                     cnt++;
 102                 }
 103                 if (gcode->has_letter('C')) {
 104                     float c = gcode->get_value('C');
 105                     theta_offset[2] -= (current_angle[2] - c);
 106                     current_angle[2]= c;
 107                     cnt++;
 108                 }
 109
 110                 // reset the actuator positions (and machine position accordingly)
 111                 // But only if all three actuators have been specified at the same time
 112                 if(cnt == 3 || (gcode->has_letter('R') && gcode->get_value('R') != 0)) {
 113                     THEKERNEL->robot->reset_actuator_position(current_angle);
 114                     gcode->stream->printf("NOTE: actuator position reset\n");
 115                 }else{
 116                     gcode->stream->printf("NOTE: actuator position NOT reset\n");
 117                 }
 118
 119                 gcode->stream->printf("Theta Offset: A %8.5f B %8.5f C %8.5f\n", theta_offset[0], theta_offset[1], theta_offset[2]);
 120             }
 121         }
 122     }
 123 }