src/modules/tools/scaracal/SCARAcal.cpp

   1 /*
   2       This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
   3       Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
   4       Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
   5       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
   6 */
   7
   8 #include "SCARAcal.h"
   9
  10 #include "Kernel.h"
  11 #include "BaseSolution.h"
  12 #include "Config.h"
  13 #include "Robot.h"
  14 #include "StepperMotor.h"
  15 #include "StreamOutputPool.h"
  16 #include "Gcode.h"
  17 #include "Conveyor.h"
  18 #include "Stepper.h"
  19 #include "checksumm.h"
  20 #include "ConfigValue.h"
  21 #include "SerialMessage.h"
  22 #include "EndstopsPublicAccess.h"
  23 #include "PublicData.h"
  24 #include <algorithm>
  25 #include <map>
  26
  27 #define scaracal_checksum CHECKSUM("scaracal")
  28 #define enable_checksum CHECKSUM("enable")
  29 #define slow_feedrate_checksum CHECKSUM("slow_feedrate")
  30 #define z_move_checksum CHECKSUM("z_move")
  31
  32 #define X_AXIS 0
  33 #define Y_AXIS 1
  34 #define Z_AXIS 2
  35
  36 #define STEPPER THEROBOT->actuators
  37 #define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm())
  38
  39 void SCARAcal::on_module_loaded()
  40 {
  41     // if the module is disabled -> do nothing
  42     if(!THEKERNEL->config->value( scaracal_checksum, enable_checksum )->by_default(false)->as_bool()) {
  43         // as this module is not needed free up the resource
  44         delete this;
  45         return;
  46     }
  47
  48     // load settings
  49     this->on_config_reload(this);
  50     // register event-handlers
  51     register_for_event(ON_GCODE_RECEIVED);
  52 }
  53
  54 void SCARAcal::on_config_reload(void *argument)
  55 {
  56     this->slow_rate = THEKERNEL->config->value( scaracal_checksum, slow_feedrate_checksum )->by_default(5)->as_number(); // feedrate in mm/sec
  57     this->z_move = THEKERNEL->config->value( scaracal_checksum, z_move_checksum )->by_default(0)->as_number(); // Optional movement of Z relative to the home position.
  58                                                                                                   // positive values increase distance between nozzle and bed.
  59                                                                                                   // negative will decrease.  Useful when level code active to prevent collision
  60
  61 }
  62
  63
  64 // issue home command
  65 void SCARAcal::home()
  66 {
  67     Gcode gc("G28", &(StreamOutput::NullStream));
  68     THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
  69 }
  70
  71 bool SCARAcal::get_trim(float& x, float& y, float& z)
  72 {
  73     void *returned_data;
  74     bool ok = PublicData::get_value( endstops_checksum, trim_checksum, &returned_data );
  75
  76     if (ok) {
  77         float *trim = static_cast<float *>(returned_data);
  78         x= trim[0];
  79         y= trim[1];
  80         z= trim[2];
  81         return true;
  82     }
  83     return false;
  84 }
  85
  86 bool SCARAcal::set_trim(float x, float y, float z, StreamOutput *stream)
  87 {
  88     float t[3]{x, y, z};
  89     bool ok= PublicData::set_value( endstops_checksum, trim_checksum, t);
  90
  91     if (ok) {
  92         stream->printf("set trim to X:%f Y:%f Z:%f\n", x, y, z);
  93     } else {
  94         stream->printf("unable to set trim, is endstops enabled?\n");
  95     }
  96
  97     return ok;
  98 }
  99
 100 bool SCARAcal::get_home_offset(float& x, float& y, float& z)
 101 {
 102     void *returned_data;
 103     bool ok = PublicData::get_value( endstops_checksum, home_offset_checksum, &returned_data );
 104
 105     if (ok) {
 106         float *home_offset = static_cast<float *>(returned_data);
 107         x= home_offset[0];
 108         y= home_offset[1];
 109         z= home_offset[2];
 110         return true;
 111     }
 112     return false;
 113 }
 114
 115 bool SCARAcal::set_home_offset(float x, float y, float z, StreamOutput *stream)
 116 {
 117     char cmd[64];
 118
 119     // Assemble Gcode to add onto the queue
 120     snprintf(cmd, sizeof(cmd), "M206 X%1.3f Y%1.3f Z%1.3f", x, y, z); // Send saved Z homing offset
 121
 122     Gcode gc(cmd, &(StreamOutput::NullStream));
 123     THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
 124
 125     stream->printf("Set home_offset to X:%f Y:%f Z:%f\n", x, y, z);
 126
 127     return true;//ok;
 128 }
 129
 130 bool SCARAcal::translate_trim(StreamOutput *stream)
 131 {
 132     float S_trim[3],
 133         home_off[3];
 134     ActuatorCoordinates actuator;
 135
 136     this->get_home_offset(home_off[0], home_off[1], home_off[2]);               // get home offsets
 137     this->get_trim(S_trim[0], S_trim[1], S_trim[2]);                              // get current trim
 138
 139     THEROBOT->arm_solution->cartesian_to_actuator( home_off, actuator ); // convert current home offset to actuator angles
 140
 141     // Subtract trim values from actuators to determine the real home offset actuator position for X and Y.
 142
 143     actuator[0] -= S_trim[0];
 144     actuator[1] -= S_trim[1];
 145
 146     // Clear X and Y trims internally
 147     S_trim[0] = 0.0F;
 148     S_trim[1] = 0.0F;
 149
 150     // convert back to get the real cartesian home offsets
 151
 152     THEROBOT->arm_solution->actuator_to_cartesian( actuator, home_off );
 153
 154     this->set_home_offset(home_off[0], home_off[1], home_off[2],stream);               // get home offsets
 155                  // Set the correct home offsets;
 156
 157     this->set_trim(S_trim[0], S_trim[1], S_trim[2], stream);    // Now Clear relevant trims
 158
 159     return true;
 160 }
 161
 162 void SCARAcal::SCARA_ang_move(float theta, float psi, float z, float feedrate)
 163 {
 164     char cmd[64];
 165     float cartesian[3];
 166     ActuatorCoordinates actuator;
 167
 168     // Assign the actuator angles from input
 169     actuator[0] = theta;
 170     actuator[1] = psi;
 171     actuator[2] = z;
 172
 173     // Calculate the physical position relating to the arm angles
 174     THEROBOT->arm_solution->actuator_to_cartesian( actuator, cartesian );
 175
 176     // Assemble Gcode to add onto the queue
 177     snprintf(cmd, sizeof(cmd), "G0 X%1.3f Y%1.3f Z%1.3f F%1.1f", cartesian[0], cartesian[1], cartesian[2], feedrate * 60); // use specified feedrate (mm/sec)
 178
 179     //THEKERNEL->streams->printf("DEBUG: move: %s\n", cmd);
 180
 181     Gcode gc(cmd, &(StreamOutput::NullStream));
 182     THEROBOT->on_gcode_received(&gc); // send to robot directly
 183 }
 184
 185 //A GCode has been received
 186 //See if the current Gcode line has some orders for us
 187 void SCARAcal::on_gcode_received(void *argument)
 188 {
 189     Gcode *gcode = static_cast<Gcode *>(argument);
 190
 191     if( gcode->has_m) {
 192         switch( gcode->m ) {
 193
 194             case 114: {    // Extra stuff for Morgan calibration
 195                 char buf[32];
 196                 float cartesian[3];
 197                 ActuatorCoordinates actuators;
 198
 199                 THEROBOT->get_axis_position(cartesian);    // get actual position from robot
 200                 THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators );      // translate to get actuator position
 201
 202                 int n = snprintf(buf, sizeof(buf), "  A: Th:%1.3f Ps:%1.3f",
 203                                  actuators[0],
 204                                  actuators[1]);    // display actuator angles Theta and Psi.
 205                 gcode->txt_after_ok.append(buf, n);
 206             }
 207             break;
 208
 209             case 360: {
 210                 float target[2] = {0.0F, 120.0F},
 211                       cartesian[3],
 212                       S_trim[3];
 213
 214                 this->get_trim(S_trim[0], S_trim[1], S_trim[2]);        // get current trim to conserve other calbration values
 215
 216                 if(gcode->has_letter('P')) {
 217                     // Program the current position as target
 218                     ActuatorCoordinates actuators;
 219                     float S_delta[2],
 220                           S_trim[3];
 221
 222                     THEROBOT->get_axis_position(cartesian);    // get actual position from robot
 223                     THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators );      // translate to get actuator position
 224
 225                     S_delta[0] = actuators[0] - target[0];
 226
 227                     set_trim(S_delta[0], S_trim[1], S_trim[2], gcode->stream);
 228                 } else {
 229                     set_trim(0, S_trim[1], S_trim[2], gcode->stream);               // reset trim for calibration move
 230                     this->home();                                                   // home
 231                     THEROBOT->get_axis_position(cartesian);    // get actual position from robot
 232                     SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target
 233                 }
 234             }
 235             break;
 236
 237             case 361: {
 238                 float target[2] = {90.0F, 130.0F},
 239                       cartesian[3];
 240
 241                 if(gcode->has_letter('P')) {
 242                     // Program the current position as target
 243                     ActuatorCoordinates actuators;
 244
 245                     THEROBOT->get_axis_position(cartesian);                                // get actual position from robot
 246                     THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
 247
 248                     STEPPER[0]->change_steps_per_mm(actuators[0] / target[0] * STEPPER[0]->get_steps_per_mm()); // Find angle difference
 249                     STEPPER[1]->change_steps_per_mm(STEPPER[0]->get_steps_per_mm());  // and change steps_per_mm to ensure correct steps per *angle*
 250                 } else {
 251                     this->home();                                                   // home - This time leave trims as adjusted.
 252                     THEROBOT->get_axis_position(cartesian);    // get actual position from robot
 253                     SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F); // move to target
 254                 }
 255
 256             }
 257             break;
 258
 259             case 364: {
 260                 float target[2] = {45.0F, 135.0F},
 261                       cartesian[3],
 262                       S_trim[3];
 263
 264                 this->get_trim(S_trim[0], S_trim[1], S_trim[2]);        // get current trim to conserve other calbration values
 265
 266                 if(gcode->has_letter('P')) {
 267                     // Program the current position as target
 268                     ActuatorCoordinates actuators;
 269                     float S_delta[2];
 270
 271                     THEROBOT->get_axis_position(cartesian);                                     // get actual position from robot
 272                     THEROBOT->arm_solution->cartesian_to_actuator( cartesian, actuators );      // translate it to get actual actuator angles
 273
 274                     S_delta[1] = ( actuators[1] - actuators[0]) - ( target[1] - target[0] );            // Find difference in angle - not actuator difference, and
 275                     set_trim(S_trim[0], S_delta[1], S_trim[2], gcode->stream);                                  // set trim to reflect the difference
 276                 } else {
 277                     set_trim(S_trim[0], 0, S_trim[2], gcode->stream);                                           // reset trim for calibration move
 278                     this->home();                                                                       // home
 279                     THEROBOT->get_axis_position(cartesian);    // get actual position from robot
 280                     SCARA_ang_move(target[0], target[1], cartesian[2] + this->z_move, slow_rate * 3.0F);                     // move to target
 281                 }
 282             }
 283             break;
 284
 285             case 366:                                       // Translate trims to the actual endstop offsets for SCARA
 286                 this->translate_trim(gcode->stream);
 287                 break;
 288
 289         }
 290     }
 291 }
 292