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
  25 #define scaracal_checksum CHECKSUM("scaracal")
  26 #define enable_checksum CHECKSUM("enable")
  27 #define slow_feedrate_checksum CHECKSUM("slow_feedrate")
  28
  29 #define X_AXIS 0
  30 #define Y_AXIS 1
  31 #define Z_AXIS 2
  32
  33 #define STEPPER THEKERNEL->robot->actuators
  34 #define STEPS_PER_MM(a) (STEPPER[a]->get_steps_per_mm())
  35
  36 void SCARAcal::on_module_loaded()
  37 {
  38     // if the module is disabled -> do nothing
  39     if(!THEKERNEL->config->value( scaracal_checksum, enable_checksum )->by_default(false)->as_bool()) {
  40         // as this module is not needed free up the resource
  41         delete this;
  42         return;
  43     }
  44
  45     // load settings
  46     this->on_config_reload(this);
  47     // register event-handlers
  48     register_for_event(ON_GCODE_RECEIVED);
  49 }
  50
  51 void SCARAcal::on_config_reload(void *argument)
  52 {
  53     this->slow_rate = THEKERNEL->config->value( scaracal_checksum, slow_feedrate_checksum )->by_default(5)->as_number(); // feedrate in mm/sec
  54
  55 }
  56
  57
  58 // issue home command
  59 void SCARAcal::home()
  60 {
  61     Gcode gc("G28", &(StreamOutput::NullStream));
  62     THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
  63 }
  64
  65 bool SCARAcal::set_trim(float x, float y, float z, StreamOutput *stream)
  66 {
  67     float t[3]{x, y, z};
  68     bool ok= PublicData::set_value( endstops_checksum, trim_checksum, t);
  69
  70     if (ok) {
  71         stream->printf("set trim to X:%f Y:%f Z:%f\n", x, y, z);
  72     } else {
  73         stream->printf("unable to set trim, is endstops enabled?\n");
  74     }
  75
  76     return ok;
  77 }
  78
  79 bool SCARAcal::get_trim(float& x, float& y, float& z)
  80 {
  81     void *returned_data;
  82     bool ok = PublicData::get_value( endstops_checksum, trim_checksum, &returned_data );
  83
  84     if (ok) {
  85         float *trim = static_cast<float *>(returned_data);
  86         x= trim[0];
  87         y= trim[1];
  88         z= trim[2];
  89         return true;
  90     }
  91     return false;
  92 }
  93
  94 void SCARAcal::SCARA_ang_move(float theta, float psi, float z, float feedrate)
  95 {
  96     char cmd[64];
  97     float actuator[3],
  98           cartesian[3];
  99
 100     // Assign the actuator angles from input
 101     actuator[0] = theta;
 102     actuator[1] = psi;
 103     actuator[2] = z;
 104
 105     // Calculate the physical position relating to the arm angles
 106     THEKERNEL->robot->arm_solution->actuator_to_cartesian( actuator, cartesian );
 107
 108     // Assemble Gcode to add onto the queue
 109     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)
 110
 111     //THEKERNEL->streams->printf("DEBUG: move: %s\n", cmd);
 112
 113     Gcode gc(cmd, &(StreamOutput::NullStream));
 114     THEKERNEL->robot->on_gcode_received(&gc); // send to robot directly
 115 }
 116
 117 //A GCode has been received
 118 //See if the current Gcode line has some orders for us
 119 void SCARAcal::on_gcode_received(void *argument)
 120 {
 121     Gcode *gcode = static_cast<Gcode *>(argument);
 122
 123     if( gcode->has_m) {
 124         switch( gcode->m ) {
 125
 126             case 114: {    // Extra stuff for Morgan calibration
 127                 char buf[32];
 128                 float cartesian[3],
 129                       actuators[3];
 130
 131                 THEKERNEL->robot->get_axis_position(cartesian);    // get actual position from robot
 132                 THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators );      // translate to get actuator position
 133
 134                 int n = snprintf(buf, sizeof(buf), "  A: Th:%1.3f Ps:%1.3f",
 135                                  actuators[0],
 136                                  actuators[1]);    // display actuator angles Theta and Psi.
 137                 gcode->txt_after_ok.append(buf, n);
 138                 gcode->mark_as_taken();
 139
 140             }
 141             return;
 142
 143             case 360: {
 144                 float target[2] = {0.0F, 120.0F},
 145                       S_trim[3];
 146
 147                 this->get_trim(S_trim[0], S_trim[1], S_trim[2]);        // get current trim to conserve other calbration values
 148
 149                 if(gcode->has_letter('P')) {
 150                     // Program the current position as target
 151                     float cartesian[3],
 152                           actuators[3],
 153                           S_delta[2],
 154                           S_trim[3];
 155
 156                     THEKERNEL->robot->get_axis_position(cartesian);    // get actual position from robot
 157                     THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators );      // translate to get actuator position
 158
 159                     S_delta[0] = actuators[0] - target[0];
 160
 161                     set_trim(S_delta[0], S_trim[1], 0, gcode->stream);
 162                 } else {
 163                     set_trim(0, S_trim[1], 0, gcode->stream);               // reset trim for calibration move
 164                     this->home();                                                   // home
 165                     SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target
 166                 }
 167                 gcode->mark_as_taken();
 168             }
 169             return;
 170             case 361: {
 171                 float target[2] = {90.0F, 130.0F};
 172                 if(gcode->has_letter('P')) {
 173                     // Program the current position as target
 174                     float cartesian[3],
 175                           actuators[3];
 176
 177                     THEKERNEL->robot->get_axis_position(cartesian);                                // get actual position from robot
 178                     THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
 179
 180                     STEPPER[0]->change_steps_per_mm(actuators[0] / target[0] * STEPPER[0]->get_steps_per_mm()); // Find angle difference
 181                     STEPPER[1]->change_steps_per_mm(STEPPER[0]->get_steps_per_mm());  // and change steps_per_mm to ensure correct steps per *angle*
 182                 } else {
 183                     this->home();                                                   // home - This time leave trims as adjusted.
 184                     SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target
 185                 }
 186                 gcode->mark_as_taken();
 187             }
 188             return;
 189               case 364: {
 190                 float target[2] = {45.0F, 135.0F},
 191                       S_trim[3];
 192
 193                 this->get_trim(S_trim[0], S_trim[1], S_trim[2]);        // get current trim to conserve other calbration values
 194
 195                 if(gcode->has_letter('P')) {
 196                     // Program the current position as target
 197                     float cartesian[3],
 198                           actuators[3],
 199                           S_delta[2];
 200
 201                     THEKERNEL->robot->get_axis_position(cartesian);                                     // get actual position from robot
 202                     THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators );      // translate it to get actual actuator angles
 203
 204                     S_delta[1] = actuators[1] - target[1];                 // Find difference, and
 205                     set_trim(S_trim[0], S_delta[1], 0, gcode->stream);     // set trim to reflect the difference
 206                 } else {
 207                     set_trim(S_trim[0], 0, 0, gcode->stream);               // reset trim for calibration move
 208                     this->home();                                                   // home
 209                     SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target
 210                 }
 211                 gcode->mark_as_taken();
 212             }
 213             return;
 214         }
 215     }
 216 }
 217