src/modules/utils/panel/screens/cnc/DirectJogScreen.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 "DirectJogScreen.h"
   9
  10 #include "libs/Kernel.h"
  11 #include "libs/SerialMessage.h"
  12 #include "Panel.h"
  13 #include "PanelScreen.h"
  14 #include "MainMenuScreen.h"
  15 #include "libs/nuts_bolts.h"
  16 #include "libs/utils.h"
  17 #include "Robot.h"
  18 #include "PublicData.h"
  19 #include "checksumm.h"
  20 #include "LcdBase.h"
  21 #include "StepperMotor.h"
  22 #include "BaseSolution.h"
  23 #include "mbed.h"
  24
  25 #include <math.h>
  26 #include <stdio.h>
  27 #include <string>
  28
  29 using namespace std;
  30
  31 DirectJogScreen::DirectJogScreen(){}
  32
  33 void DirectJogScreen::on_enter()
  34 {
  35     THEPANEL->enter_menu_mode();
  36     mode= MULTIPLIER;
  37     THEPANEL->setup_menu(7);
  38     this->refresh_menu();
  39     this->pos_changed = false;
  40     get_actuator_pos();
  41 }
  42
  43 // called in on_idle()
  44 void DirectJogScreen::on_refresh()
  45 {
  46     if ( THEPANEL->menu_change() ) {
  47         this->refresh_menu();
  48     }
  49
  50     if ( THEPANEL->click() ) {
  51         switch(mode) {
  52             case JOG:
  53                 THEROBOT->reset_position_from_current_actuator_position(); // needed as we were changing the actuator only
  54                 this->enter_menu_control();
  55                 this->refresh_menu();
  56                 break;
  57
  58             case MULTIPLIER:
  59             case AXIS_SELECT:
  60                 this->clicked_menu_entry(THEPANEL->get_menu_current_line());
  61                 break;
  62         }
  63
  64     } else if(pos_changed) {
  65         pos_changed= false;
  66         get_actuator_pos();
  67         THEPANEL->lcd->setCursor(0, 2);
  68         this->display_axis_line(this->axis);
  69     }
  70
  71 }
  72
  73 void DirectJogScreen::display_menu_line(uint16_t line)
  74 {
  75     // in menu mode
  76     if(mode == MULTIPLIER) {
  77         switch ( line ) {
  78             case 0: THEPANEL->lcd->printf("Back");  break;
  79             case 1: THEPANEL->lcd->printf("x1"); break;
  80             case 2: THEPANEL->lcd->printf("x4"); break;
  81             case 3: THEPANEL->lcd->printf("x8"); break;
  82             case 4: THEPANEL->lcd->printf("x16"); break;
  83             case 5: THEPANEL->lcd->printf("x32"); break;
  84             case 6: THEPANEL->lcd->printf("x64"); break;
  85         }
  86
  87     }else if(mode == AXIS_SELECT){
  88         switch ( line ) {
  89             case 0: THEPANEL->lcd->printf("Back");  break;
  90             case 1: this->display_axis_line(X_AXIS); break;
  91             case 2: this->display_axis_line(Y_AXIS); break;
  92             case 3: this->display_axis_line(Z_AXIS); break;
  93         }
  94     }
  95 }
  96
  97 void DirectJogScreen::display_axis_line(uint8_t axis)
  98 {
  99     THEPANEL->lcd->printf("%c    %8.3f", 'X' + axis, this->pos[axis]);
 100 }
 101
 102
 103 void DirectJogScreen::clicked_menu_entry(uint16_t line)
 104 {
 105     if(mode == MULTIPLIER) {
 106         switch ( line ) {
 107             case 0: THEPANEL->enter_screen(this->parent); break;
 108             case 1: multiplier= 1; break;
 109             case 2: multiplier= 4; break;
 110             case 3: multiplier= 8; break;
 111             case 4: multiplier= 16; break;
 112             case 5: multiplier= 32; break;
 113             case 6: multiplier= 64; break;
 114         }
 115
 116         if(line > 0) {
 117             enter_menu_control();
 118         }
 119
 120     }else if(mode == AXIS_SELECT){
 121         switch ( line ) {
 122             case 0: THEPANEL->enter_screen(this->parent); break;
 123             case 1: this->enter_axis_control(X_AXIS); break;
 124             case 2: this->enter_axis_control(Y_AXIS); break;
 125             case 3: this->enter_axis_control(Z_AXIS); break;
 126         }
 127     }
 128 }
 129
 130 void DirectJogScreen::on_exit()
 131 {
 132     delete this;
 133 }
 134
 135 void DirectJogScreen::get_actuator_pos()
 136 {
 137         // get real time positions
 138         ActuatorCoordinates current_position{
 139             THEROBOT->actuators[X_AXIS]->get_current_position(),
 140             THEROBOT->actuators[Y_AXIS]->get_current_position(),
 141             THEROBOT->actuators[Z_AXIS]->get_current_position()
 142         };
 143
 144         // get machine position from the actuator position using FK
 145         float mpos[3];
 146         THEROBOT->arm_solution->actuator_to_cartesian(current_position, mpos);
 147         Robot::wcs_t wpos= THEROBOT->mcs2wcs(mpos);
 148         this->pos[0]= THEROBOT->from_millimeters(std::get<X_AXIS>(wpos));
 149         this->pos[1]= THEROBOT->from_millimeters(std::get<Y_AXIS>(wpos));
 150         this->pos[2]= THEROBOT->from_millimeters(std::get<Z_AXIS>(wpos));
 151 }
 152
 153 // encoder tick, called in an interrupt everytime we get an encoder tick
 154 // fastest is 200pulses/sec
 155 void DirectJogScreen::tick(int change)
 156 {
 157     for (int i = 0; i < multiplier; ++i) {
 158         if(i != 0) wait_us(100);
 159        THEROBOT->actuators[axis]->manual_step(change < 0);
 160     }
 161     pos_changed= true;
 162 }
 163
 164 void DirectJogScreen::enter_axis_control(uint8_t axis)
 165 {
 166     this->mode = JOG;
 167     this->axis = axis;
 168     THEPANEL->lcd->clear();
 169     THEPANEL->lcd->setCursor(0, 0);
 170     THEPANEL->lcd->printf("MPG mode x%d", multiplier);
 171     THEPANEL->lcd->setCursor(0, 2);
 172     get_actuator_pos();
 173     this->display_axis_line(this->axis);
 174
 175     using std::placeholders::_1;
 176     THEPANEL->enter_direct_encoder_mode(std::bind(&DirectJogScreen::tick, this, _1));
 177 }
 178
 179 void DirectJogScreen::enter_menu_control()
 180 {
 181     this->mode = AXIS_SELECT;
 182     THEPANEL->setup_menu(4);
 183     THEPANEL->enter_menu_mode(true);
 184 }