From f9a0f86dec813b854b1a82951ee0537fcb9749e0 Mon Sep 17 00:00:00 2001 From: Jim Morris Date: Tue, 26 Apr 2016 18:05:27 -0700 Subject: [PATCH] get direct step running from encoder --- src/libs/StepTicker.cpp | 7 + src/libs/StepTicker.h | 1 + src/libs/StepperMotor.cpp | 23 +++ src/libs/StepperMotor.h | 4 +- src/modules/utils/panel/Panel.cpp | 43 +++-- src/modules/utils/panel/Panel.h | 9 +- .../utils/panel/screens/cnc/ControlScreen.cpp | 6 +- .../panel/screens/cnc/DirectJogScreen.cpp | 150 ++++++++++++++++++ .../utils/panel/screens/cnc/DirectJogScreen.h | 37 +++++ .../utils/panel/screens/cnc/JogScreen.cpp | 20 ++- 10 files changed, 280 insertions(+), 20 deletions(-) create mode 100644 src/modules/utils/panel/screens/cnc/DirectJogScreen.cpp create mode 100644 src/modules/utils/panel/screens/cnc/DirectJogScreen.h diff --git a/src/libs/StepTicker.cpp b/src/libs/StepTicker.cpp index 75be985b..3a8f62e8 100644 --- a/src/libs/StepTicker.cpp +++ b/src/libs/StepTicker.cpp @@ -186,6 +186,13 @@ void StepTicker::acceleration_tick() { } } +void StepTicker::schedule_unstep(int motor) +{ + this->unstep[motor]= 1; + LPC_TIM1->TCR = 3; + LPC_TIM1->TCR = 1; +} + void StepTicker::TIMER0_IRQHandler (void){ // Reset interrupt register LPC_TIM0->IR |= 1 << 0; diff --git a/src/libs/StepTicker.h b/src/libs/StepTicker.h index d06796fa..32caaaa8 100644 --- a/src/libs/StepTicker.h +++ b/src/libs/StepTicker.h @@ -43,6 +43,7 @@ class StepTicker{ } void acceleration_tick(); void synchronize_acceleration(bool fire_now); + void schedule_unstep(int motor); void start(); diff --git a/src/libs/StepperMotor.cpp b/src/libs/StepperMotor.cpp index d671096f..f4b1d23c 100644 --- a/src/libs/StepperMotor.cpp +++ b/src/libs/StepperMotor.cpp @@ -11,6 +11,7 @@ #include "StepTicker.h" #include +#include "mbed.h" // in steps/sec the default minimum speed (was 20steps/sec hardcoded) float StepperMotor::default_minimum_actuator_rate= 20.0F; @@ -97,6 +98,28 @@ void StepperMotor::step() } } +// Does a manual step pulse, used for direct encoder control of a stepper +void StepperMotor::manual_step(bool dir) +{ + if(!enabled) enable(true); + + // set direction if needed + if(this->direction != dir) { + this->direction= dir; + this->dir_pin.set(dir); + wait_us(1); + } + + // set step pin + this->step_pin.set(1); + + // schedule for unstep + THEKERNEL->step_ticker->schedule_unstep(this->index); + + // keep track of actuators actual position in steps + this->current_position_steps += (dir ? -1 : 1); +} + // If the move is finished, the StepTicker will call this ( because we asked it to in tick() ) void StepperMotor::signal_move_finished() { diff --git a/src/libs/StepperMotor.h b/src/libs/StepperMotor.h index 78fa8724..0f1ede63 100644 --- a/src/libs/StepperMotor.h +++ b/src/libs/StepperMotor.h @@ -24,8 +24,9 @@ class StepperMotor { void step(); inline void unstep() { step_pin.set(0); }; + void manual_step(bool dir); - inline void enable(bool state) { en_pin.set(!state); }; + inline void enable(bool state) { enabled= state; en_pin.set(!state); }; bool is_moving() const { return moving; } bool which_direction() const { return direction; } @@ -100,6 +101,7 @@ class StepperMotor { volatile bool force_finish:1; // set to force a move to finish early bool direction:1; bool last_step_tick_valid:1; // set if the last step tick time is valid (ie the motor moved last block) + bool enabled:1; }; // Called a great many times per second, to step if we have to now diff --git a/src/modules/utils/panel/Panel.cpp b/src/modules/utils/panel/Panel.cpp index 54634912..4e2b738d 100644 --- a/src/modules/utils/panel/Panel.cpp +++ b/src/modules/utils/panel/Panel.cpp @@ -67,6 +67,10 @@ Panel* Panel::instance= nullptr; +#define MENU_MODE 0 +#define CONTROL_MODE 1 +#define DIRECT_ENCODER_MODE 2 + Panel::Panel() { instance= this; @@ -230,7 +234,16 @@ uint32_t Panel::encoder_check(uint32_t dummy) // NOTE FIXME (WHY is it in menu only?) this code will not work if change is not 1,0,-1 anything greater (as in above case) will not work properly static int encoder_counter = 0; // keeps track of absolute encoder position static int last_encoder_click= 0; // last readfing of divided encoder count + int change = lcd->readEncoderDelta(); + + if(mode == DIRECT_ENCODER_MODE) { + if(change != 0 && encoder_cb_fnc) { + encoder_cb_fnc(change); + } + return 0; + } + encoder_counter += change; int clicks= encoder_counter/this->encoder_click_resolution; int delta= clicks - last_encoder_click; // the number of clicks this time @@ -251,13 +264,21 @@ uint32_t Panel::button_tick(uint32_t dummy) return 0; } -// Read and update encoder +// Read and update encoder from a slow source uint32_t Panel::encoder_tick(uint32_t dummy) { this->do_encoder = true; return 0; } +// special mode where all encoder ticks are sent to the given function +bool Panel::enter_direct_encoder_mode(encoder_cb_t fnc) +{ + encoder_cb_fnc= fnc; + this->mode= DIRECT_ENCODER_MODE; + return true; +} + void Panel::on_set_public_data(void *argument) { PublicDataRequest *pdr = static_cast(argument); @@ -372,14 +393,15 @@ void Panel::on_idle(void *argument) this->click_button.check_signal(but & BUTTON_SELECT); } - // If we are in menu mode and the position has changed - if ( this->mode == MENU_MODE && this->counter_change() ) { - this->menu_update(); - } - - // If we are in control mode - if ( this->mode == CONTROL_MODE && this->counter_change() ) { - this->control_value_update(); + if(this->counter_change()) { + switch(this->mode) { + case MENU_MODE: // If we are in menu mode and the position has changed + this->menu_update(); + break; + case CONTROL_MODE: // If we are in control mode + this->control_value_update(); + break; + } } // If we must refresh @@ -449,13 +471,13 @@ bool Panel::click() } } - // Enter menu mode void Panel::enter_menu_mode(bool force) { this->mode = MENU_MODE; this->counter = &this->menu_selected_line; this->menu_changed = force; + encoder_cb_fnc= nullptr; } void Panel::setup_menu(uint16_t rows) @@ -553,6 +575,7 @@ bool Panel::enter_control_mode(float passed_normal_increment, float passed_press this->counter = &this->control_normal_counter; this->control_normal_counter = 0; this->control_base_value = 0; + encoder_cb_fnc= nullptr; return true; } diff --git a/src/modules/utils/panel/Panel.h b/src/modules/utils/panel/Panel.h index b47fb737..8af40349 100644 --- a/src/modules/utils/panel/Panel.h +++ b/src/modules/utils/panel/Panel.h @@ -12,10 +12,9 @@ #include "Pin.h" #include "mbed.h" #include -using std::string; +#include -#define MENU_MODE 0 -#define CONTROL_MODE 1 +using std::string; #define THEPANEL Panel::instance @@ -87,6 +86,9 @@ class Panel : public Module { LcdBase* lcd; PanelScreen* custom_screen; + using encoder_cb_t= std::function; + bool enter_direct_encoder_mode(encoder_cb_t fnc); + // as panelscreen accesses private fields in Panel friend class PanelScreen; @@ -127,6 +129,7 @@ class Panel : public Module { float default_bed_temperature; string message; + encoder_cb_t encoder_cb_fnc; uint16_t screen_lines; uint16_t menu_current_line; diff --git a/src/modules/utils/panel/screens/cnc/ControlScreen.cpp b/src/modules/utils/panel/screens/cnc/ControlScreen.cpp index 51dc9c0d..a43d6610 100644 --- a/src/modules/utils/panel/screens/cnc/ControlScreen.cpp +++ b/src/modules/utils/panel/screens/cnc/ControlScreen.cpp @@ -26,7 +26,6 @@ using namespace std; #define NULL_CONTROL_MODE 0 #define AXIS_CONTROL_MODE 1 -#define INCREMENT_SELECTION_MODE 2 ControlScreen::ControlScreen() { @@ -37,7 +36,7 @@ void ControlScreen::on_enter() { THEPANEL->enter_menu_mode(); THEPANEL->setup_menu(4); - get_current_pos(this->pos); + get_current_pos(this->pos); // gets current WCS this->refresh_menu(); this->pos_changed = false; } @@ -113,6 +112,9 @@ void ControlScreen::enter_axis_control(char axis) THEPANEL->enter_control_mode(this->jog_increment, this->jog_increment / 10); THEPANEL->set_control_value(this->pos[axis - 'X']); THEPANEL->lcd->clear(); + THEPANEL->lcd->setCursor(0, 0); + THEPANEL->lcd->printf("Jog %6.2f", jog_increment); + THEPANEL->lcd->setCursor(0, 2); this->display_axis_line(this->controlled_axis); } diff --git a/src/modules/utils/panel/screens/cnc/DirectJogScreen.cpp b/src/modules/utils/panel/screens/cnc/DirectJogScreen.cpp new file mode 100644 index 00000000..986bbb64 --- /dev/null +++ b/src/modules/utils/panel/screens/cnc/DirectJogScreen.cpp @@ -0,0 +1,150 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + 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. + 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. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +#include "DirectJogScreen.h" + +#include "libs/Kernel.h" +#include "libs/SerialMessage.h" +#include "Panel.h" +#include "PanelScreen.h" +#include "MainMenuScreen.h" +#include "libs/nuts_bolts.h" +#include "libs/utils.h" +#include "Robot.h" +#include "PublicData.h" +#include "checksumm.h" +#include "LcdBase.h" +#include "StepperMotor.h" +#include "BaseSolution.h" + +#include +#include +#include + +using namespace std; + +DirectJogScreen::DirectJogScreen() +{ + mode= AXIS_SELECT; +} + +void DirectJogScreen::on_enter() +{ + THEPANEL->enter_menu_mode(); + THEPANEL->setup_menu(4); + get_current_pos(this->pos); + this->refresh_menu(); + this->pos_changed = false; + mode= AXIS_SELECT; +} + +// called in on_idle() +void DirectJogScreen::on_refresh() +{ + if ( THEPANEL->menu_change() ) { + this->refresh_menu(); + } + + if ( THEPANEL->click() ) { + switch(mode) { + case JOG: + this->enter_menu_control(); + this->refresh_menu(); + break; + + case AXIS_SELECT: + this->clicked_menu_entry(THEPANEL->get_menu_current_line()); + break; + } + + } else if(pos_changed) { + pos_changed= false; + // get real time positions + ActuatorCoordinates current_position{ + THEKERNEL->robot->actuators[X_AXIS]->get_current_position(), + THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(), + THEKERNEL->robot->actuators[Z_AXIS]->get_current_position() + }; + + // get machine position from the actuator position using FK + float mpos[3]; + THEKERNEL->robot->arm_solution->actuator_to_cartesian(current_position, mpos); + Robot::wcs_t wpos= THEKERNEL->robot->mcs2wcs(mpos); + this->pos[0]= THEKERNEL->robot->from_millimeters(std::get(wpos)); + this->pos[1]= THEKERNEL->robot->from_millimeters(std::get(wpos)); + this->pos[2]= THEKERNEL->robot->from_millimeters(std::get(wpos)); + + THEPANEL->lcd->setCursor(0, 2); + this->display_axis_line(this->axis); + } + +} + +void DirectJogScreen::display_menu_line(uint16_t line) +{ + // in menu mode + switch ( line ) { + case 0: THEPANEL->lcd->printf("Back"); break; + case 1: this->display_axis_line(X_AXIS); break; + case 2: this->display_axis_line(Y_AXIS); break; + case 3: this->display_axis_line(Z_AXIS); break; + } +} + +void DirectJogScreen::display_axis_line(uint8_t axis) +{ + THEPANEL->lcd->printf("%c %8.3f", 'X' + axis, this->pos[axis]); +} + + +void DirectJogScreen::clicked_menu_entry(uint16_t line) +{ + switch ( line ) { + case 0: THEPANEL->enter_screen(this->parent); break; + case 1: this->enter_axis_control(X_AXIS); break; + case 2: this->enter_axis_control(Y_AXIS); break; + case 3: this->enter_axis_control(Z_AXIS); break; + } +} + +void DirectJogScreen::on_exit() +{ + delete this; +} + +// encoder tick, called in an interrupt everytime we get an encoder tick +void DirectJogScreen::tick(int change) +{ + int steps= std::abs(change); // TODO may need multiplier here for regular encoder, or do speed check and increase rate accordingly + for (int i = 0; i < steps; ++i) { + // foreach tick we issue a step to the selected axis + if(i > 0) wait_us(10); // fastest rate is 100KHz + THEKERNEL->robot->actuators[axis]->manual_step(change < 0); + } + + pos_changed= true; +} + +void DirectJogScreen::enter_axis_control(uint8_t axis) +{ + this->mode = JOG; + this->axis = axis; + THEPANEL->lcd->clear(); + THEPANEL->lcd->setCursor(0, 0); + THEPANEL->lcd->printf("Encoder Jog Control"); + THEPANEL->lcd->setCursor(0, 2); + this->display_axis_line(this->axis); + + using std::placeholders::_1; + THEPANEL->enter_direct_encoder_mode(std::bind(&DirectJogScreen::tick, this, _1)); +} + +void DirectJogScreen::enter_menu_control() +{ + this->mode = AXIS_SELECT; + THEPANEL->enter_menu_mode(); +} diff --git a/src/modules/utils/panel/screens/cnc/DirectJogScreen.h b/src/modules/utils/panel/screens/cnc/DirectJogScreen.h new file mode 100644 index 00000000..ffef0181 --- /dev/null +++ b/src/modules/utils/panel/screens/cnc/DirectJogScreen.h @@ -0,0 +1,37 @@ +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + 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. + 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. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + +#pragma once + +#include "PanelScreen.h" + +class DirectJogScreen : public PanelScreen +{ +public: + DirectJogScreen(); + void on_refresh(); + void on_enter(); + void on_exit(); + void on_main_loop() {} + + void display_menu_line(uint16_t line); + int idle_timeout_secs() { return 120; } + +private: + void clicked_menu_entry(uint16_t line); + void display_axis_line(uint8_t axis); + void enter_axis_control(uint8_t axis); + void enter_menu_control(); + void tick(int change); + + float pos[3]; + + enum MODE_T { JOG, AXIS_SELECT }; + MODE_T mode; + uint8_t axis; + bool pos_changed; +}; diff --git a/src/modules/utils/panel/screens/cnc/JogScreen.cpp b/src/modules/utils/panel/screens/cnc/JogScreen.cpp index d36c643a..a3247b32 100644 --- a/src/modules/utils/panel/screens/cnc/JogScreen.cpp +++ b/src/modules/utils/panel/screens/cnc/JogScreen.cpp @@ -12,6 +12,7 @@ #include "LcdBase.h" #include "MainMenuScreen.h" #include "JogScreen.h" +#include "DirectJogScreen.h" #include "ControlScreen.h" #include "libs/nuts_bolts.h" #include "libs/utils.h" @@ -29,7 +30,7 @@ JogScreen::JogScreen() void JogScreen::on_enter() { THEPANEL->enter_menu_mode(); - THEPANEL->setup_menu(5); + THEPANEL->setup_menu(6); this->refresh_menu(); } @@ -48,20 +49,31 @@ void JogScreen::display_menu_line(uint16_t line) switch ( line ) { case 0: THEPANEL->lcd->printf("Back"); break; case 1: THEPANEL->lcd->printf("Move 10.0mm \x7E"); break; - case 2: THEPANEL->lcd->printf("Move 1.0mm \x7E"); break; - case 3: THEPANEL->lcd->printf("Move 0.1mm \x7E"); break; + case 2: THEPANEL->lcd->printf("Move 1.0mm \x7E"); break; + case 3: THEPANEL->lcd->printf("Move 0.1mm \x7E"); break; case 4: THEPANEL->lcd->printf("Move 0.01mm \x7E"); break; + case 5: THEPANEL->lcd->printf("Direct Control \x7E"); break; } } void JogScreen::clicked_menu_entry(uint16_t line) { + bool direct= false; switch ( line ) { case 0: THEPANEL->enter_screen(this->parent); return; case 1: this->control_screen->set_jog_increment(10.0); break; case 2: this->control_screen->set_jog_increment(1.0); break; case 3: this->control_screen->set_jog_increment(0.1); break; case 4: this->control_screen->set_jog_increment(0.01); break; + case 5: direct= true; break; + } + + if(direct) { + auto djs= new DirectJogScreen(); + djs->set_parent(this); + THEPANEL->enter_screen(djs); // self deleting + + }else{ + THEPANEL->enter_screen(this->control_screen); } - THEPANEL->enter_screen(this->control_screen); } -- 2.20.1