From 40cf991d4c755a2c5057281ce9e50abfb790ffa1 Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Tue, 30 Jul 2024 00:04:54 +0300 Subject: [PATCH 01/14] Fix timerBegin for espressif_3.0 h4\h4.ino:570:37: error: too many arguments to function 'hw_timer_t* timerBegin(uint32_t)' 570 | hw_timer_t async_timer = timerBegin(0, 80, true); | ~~~~~~~~~~^~~~~~~~~~~~~ 'hw_timer_t timerBegin(uint32_t)' --- h4/h4.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/h4/h4.ino b/h4/h4.ino index e20dc5e..d11a77c 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -567,7 +567,7 @@ int gcodeProgramCount = 0; String gcodeProgram = ""; int gcodeProgramCharIndex = 0; -hw_timer_t *async_timer = timerBegin(0, 80, true); +hw_timer_t *async_timer = timerBegin(80); bool timerAttached = false; int getApproxRpm() { From be03cd0b6ceaf594cf622a1b6b35211fb3e0d962 Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Tue, 30 Jul 2024 01:33:38 +0300 Subject: [PATCH 02/14] Fix timerBegin for espressif_3.0 --- h4/h4.ino | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/h4/h4.ino b/h4/h4.ino index d11a77c..89cca1d 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -1039,9 +1039,9 @@ void IRAM_ATTR pulse2Enc() { void setAsyncTimerEnable(bool value) { if (value) { - timerAlarmEnable(async_timer); + timerStart(async_timer); } else { - timerAlarmDisable(async_timer); + timerStop(async_timer); } } @@ -1862,9 +1862,7 @@ void updateAsyncTimerSettings() { setDir(getAsyncAxis(), dupr > 0); // dupr can change while we're in async mode, keep updating timer frequency. - timerAlarmWrite(async_timer, getTimerLimit(), true); - // without this timer stops working if already above new limit - timerWrite(async_timer, 0); + timerAlarm(async_timer, getTimerLimit(), true, 0); } void setDupr(long value) { @@ -1965,7 +1963,7 @@ void setModeFromLoop(int value) { if (mode == MODE_ASYNC || mode == MODE_A1) { if (!timerAttached) { timerAttached = true; - timerAttachInterrupt(async_timer, &onAsyncTimer, true); + timerAttachInterrupt(async_timer, &onAsyncTimer); } updateAsyncTimerSettings(); setAsyncTimerEnable(true); From 874afb16a5d0519893a12245887aa336a38e5e7c Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Tue, 30 Jul 2024 01:34:22 +0300 Subject: [PATCH 03/14] pref fix? --- h4/h4.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/h4/h4.ino b/h4/h4.ino index 89cca1d..217f652 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -1666,7 +1666,7 @@ void setup() { isOn = false; savedDupr = dupr = pref.getLong(PREF_DUPR); motionMutex = xSemaphoreCreateMutex(); - savedStarts = starts = min(STARTS_MAX, max(1, pref.getInt(PREF_STARTS))); + savedStarts = starts = min(static_cast(STARTS_MAX), max(static_cast(1), pref.getInt(PREF_STARTS))); z.savedPos = z.pos = pref.getLong(PREF_POS_Z); z.savedPosGlobal = z.posGlobal = pref.getLong(PREF_POS_GLOBAL_Z); z.savedOriginPos = z.originPos = pref.getLong(PREF_ORIGIN_POS_Z); From a4336cf14e7893490d6bc2fdbb1bf11481f8635e Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Tue, 30 Jul 2024 02:49:05 +0300 Subject: [PATCH 04/14] Update .gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 7fde7d9..e35e146 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ */debug.cfg */esp32s3.svd h4/single_axis_manual* +h4/.theia/launch.json From 7e7becb9fa459d4d101a84bd5f6adfb5ab4a268e Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Tue, 30 Jul 2024 14:13:53 +0300 Subject: [PATCH 05/14] LiquidCrystal_I2C --- h4/h4.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/h4/h4.ino b/h4/h4.ino index 217f652..aa783e1 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -246,8 +246,8 @@ const float GCODE_FEED_MIN_DU_SEC = 167; // Minimum feed in du/sec in GCode mode #include #include -#include -LiquidCrystal lcd(21, 48, 47, 38, 39, 40, 41, 42, 2, 1); +#include +LiquidCrystal_I2C lcd(0x27, 20, 4); #define LCD_HASH_INITIAL -3845709 // Random number that's unlikely to naturally occur as an actual hash long lcdHashLine0 = LCD_HASH_INITIAL; long lcdHashLine1 = LCD_HASH_INITIAL; From a999d4e399a1c753c878d01126694f3818e65e4b Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Tue, 30 Jul 2024 16:38:17 +0300 Subject: [PATCH 06/14] LiquidCrystal_I2C --- h4/h4.ino | 1 + 1 file changed, 1 insertion(+) diff --git a/h4/h4.ino b/h4/h4.ino index aa783e1..f096f0b 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -354,6 +354,7 @@ struct Axis { void initAxis(Axis* a, char name, bool active, bool rotational, float motorSteps, float screwPitch, long speedStart, long speedManualMove, long acceleration, bool invertStepper, bool needsRest, long maxTravelMm, long backlashDu, int ena, int dir, int step) { + Wire.begin(SDA, SCL); a->mutex = xSemaphoreCreateMutex(); a->name = name; From 4f9fa2c924c0f17eee284444511a3ac83e4938c2 Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Tue, 30 Jul 2024 16:41:28 +0300 Subject: [PATCH 07/14] lcd.backlight --- h4/h4.ino | 2 ++ 1 file changed, 2 insertions(+) diff --git a/h4/h4.ino b/h4/h4.ino index f096f0b..18d500f 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -355,6 +355,8 @@ struct Axis { void initAxis(Axis* a, char name, bool active, bool rotational, float motorSteps, float screwPitch, long speedStart, long speedManualMove, long acceleration, bool invertStepper, bool needsRest, long maxTravelMm, long backlashDu, int ena, int dir, int step) { Wire.begin(SDA, SCL); + lcd.init(); // initialize the lcd + lcd.backlight(); a->mutex = xSemaphoreCreateMutex(); a->name = name; From f648443066664b0550e12db800ae24d1a8e0093a Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Fri, 9 Aug 2024 04:44:51 +0300 Subject: [PATCH 08/14] Update h4.ino --- h4/h4.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/h4/h4.ino b/h4/h4.ino index 18d500f..102bc14 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -94,7 +94,7 @@ const int GCODE_MIN_RPM = 30; // pause GCode execution if RPM is below this #define SOFTWARE_VERSION 12 // To be changed whenever a different PCB / encoder / stepper / ... design is used. -#define HARDWARE_VERSION 4 +#define HARDWARE_VERSION 5 #define Z_ENA 16 #define Z_DIR 17 From 1fa3b1e70a38b58bfea814e320f5cd479fc73999 Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Sun, 11 Aug 2024 01:06:42 +0300 Subject: [PATCH 09/14] Timer fixes --- h4/h4.ino | 2 ++ 1 file changed, 2 insertions(+) diff --git a/h4/h4.ino b/h4/h4.ino index 102bc14..22f1ac9 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -1866,6 +1866,8 @@ void updateAsyncTimerSettings() { // dupr can change while we're in async mode, keep updating timer frequency. timerAlarm(async_timer, getTimerLimit(), true, 0); + // without this timer stops working if already above new limit + timerWrite(async_timer, 0); } void setDupr(long value) { From f26c27f7c38fc973a4854dd6db5d6af561866a14 Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Sun, 11 Aug 2024 04:20:16 +0300 Subject: [PATCH 10/14] keycode info --- h4/h4.ino | 58 ++++++++++++++++++++++++++++++++----------------------- 1 file changed, 34 insertions(+), 24 deletions(-) diff --git a/h4/h4.ino b/h4/h4.ino index 22f1ac9..799af7c 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -115,24 +115,34 @@ const int GCODE_MIN_RPM = 30; // pause GCode execution if RPM is below this #define A21 12 #define A22 13 #define A23 14 - +//code keypad +/* + C0 C1 C2 C3 C4 C5 C6 +R0 1 2 3 4 5 6 7 +R1 11 12 13 14 15 16 17 +R2 21 22 23 24 25 26 27 +R3 31 32 33 34 35 36 37 +R4 41 42 43 44 45 46 47 +R5 51 52 53 54 55 56 57 +R6 61 62 63 64 65 66 67 +*/ #define B_LEFT 57 #define B_RIGHT 37 #define B_UP 47 #define B_DOWN 67 #define B_MINUS 5 #define B_PLUS 64 -#define B_ON 17 -#define B_OFF 27 -#define B_STOPL 7 -#define B_STOPR 15 -#define B_STOPU 6 -#define B_STOPD 16 -#define B_DISPL 14 -#define B_STEP 24 -#define B_SETTINGS 34 -#define B_MEASURE 54 -#define B_REVERSE 44 +#define B_ON 17 //START +#define B_OFF 27 //STOP +#define B_STOPL 7 //LLEFT +#define B_STOPR 15 //LRIGHT +#define B_STOPU 6 //LUP +#define B_STOPD 16 //LDOWN +#define B_DISPL 14 //M5 +#define B_STEP 24 //M4 +#define B_SETTINGS 34 //M3 +#define B_MEASURE 54 //M1 +#define B_REVERSE 44 //M2 #define B_0 51 #define B_1 41 #define B_2 61 @@ -143,18 +153,18 @@ const int GCODE_MIN_RPM = 30; // pause GCode execution if RPM is below this #define B_7 11 #define B_8 22 #define B_9 1 -#define B_BACKSPACE 32 -#define B_MODE_GEARS 42 -#define B_MODE_TURN 52 -#define B_MODE_FACE 62 -#define B_MODE_CONE 3 -#define B_MODE_CUT 13 -#define B_MODE_THREAD 23 -#define B_MODE_OTHER 33 -#define B_X 53 -#define B_Z 43 -#define B_A 4 -#define B_B 63 +#define B_BACKSPACE 32 //BS +#define B_MODE_GEARS 42 //F1 +#define B_MODE_TURN 52 //F2 +#define B_MODE_FACE 62 //F3 +#define B_MODE_CONE 3 //F4 +#define B_MODE_CUT 13 //F5 +#define B_MODE_THREAD 23 //F6 +#define B_MODE_OTHER 33 //F7 +#define B_X 53 //M8 +#define B_Z 43 //M9 +#define B_A 4 //M6 +#define B_B 63 //M7 #define PREF_VERSION "v" #define PREF_DUPR "d" From ef0fbc077536a6765beacd6bc6bb9705786db6b7 Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Fri, 16 Aug 2024 03:27:30 +0300 Subject: [PATCH 11/14] int32_t --- h4/h4.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/h4/h4.ino b/h4/h4.ino index 799af7c..4e55609 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -71,7 +71,7 @@ const int ENCODER_FILTER = 2; // Encoder pulses shorter than this will be ignore const int PCNT_LIM = 31000; // Limit used in hardware pulse counter logic. const int PCNT_CLEAR = 30000; // Limit where we reset hardware pulse counter value to avoid overflow. Less than PCNT_LIM. const long DUPR_MAX = 254000; // No more than 1 inch pitch -const int STARTS_MAX = 124; // No more than 124-start thread +const int32_t STARTS_MAX = 124; // No more than 124-start thread const long PASSES_MAX = 999; // No more turn or face passes than this const long SAFE_DISTANCE_DU = 5000; // Step back 0.5mm from the material when moving between cuts in automated modes const long SAVE_DELAY_US = 5000000; // Wait 5s after last save and last change of saveable data before saving again @@ -1679,7 +1679,7 @@ void setup() { isOn = false; savedDupr = dupr = pref.getLong(PREF_DUPR); motionMutex = xSemaphoreCreateMutex(); - savedStarts = starts = min(static_cast(STARTS_MAX), max(static_cast(1), pref.getInt(PREF_STARTS))); + savedStarts = starts = min(STARTS_MAX, max(static_cast(1), pref.getInt(PREF_STARTS))); z.savedPos = z.pos = pref.getLong(PREF_POS_Z); z.savedPosGlobal = z.posGlobal = pref.getLong(PREF_POS_GLOBAL_Z); z.savedOriginPos = z.originPos = pref.getLong(PREF_ORIGIN_POS_Z); From 0a22b19564e54deb087a1a6aa32bcd3b93fd0d03 Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Mon, 4 Nov 2024 13:37:52 +0300 Subject: [PATCH 12/14] 141254 --- h4/LiquidCrystal_I2C_pt.cpp | 338 ++++++++++++++++++++++++++++++++++++ h4/LiquidCrystal_I2C_pt.h | 137 +++++++++++++++ h4/h4.ino | 335 ++++++++++++++++++++--------------- 3 files changed, 673 insertions(+), 137 deletions(-) create mode 100644 h4/LiquidCrystal_I2C_pt.cpp create mode 100644 h4/LiquidCrystal_I2C_pt.h diff --git a/h4/LiquidCrystal_I2C_pt.cpp b/h4/LiquidCrystal_I2C_pt.cpp new file mode 100644 index 0000000..3a51b09 --- /dev/null +++ b/h4/LiquidCrystal_I2C_pt.cpp @@ -0,0 +1,338 @@ +// Based on the work by DFRobot +/* 2024-06-24: + * based on https://github.com/johnrickman/LiquidCrystal_I2C + * added passing of an TwoWire pointer for Controllers with more + * Interfaces and changable pin's + */ + +#include "LiquidCrystal_I2C_pt.h" +#include + +#if defined(ARDUINO) && ARDUINO >= 100 +#include "Arduino.h" + +#define printIIC(args) _wire_ptr->write(args) +inline size_t LiquidCrystal_I2C::write(uint8_t value) { + send(value, Rs); + return 1; +} + +#else +#include "WProgram.h" + +#define printIIC(args) _wire_ptr->send(args) +inline void LiquidCrystal_I2C::write(uint8_t value) { + send(value, Rs); +} + +#endif + + + + +// When the display powers up, it is configured as follows: +// +// 1. Display clear +// 2. Function set: +// DL = 1; 8-bit interface data +// N = 0; 1-line display +// F = 0; 5x8 dot character font +// 3. Display on/off control: +// D = 0; Display off +// C = 0; Cursor off +// B = 0; Blinking off +// 4. Entry mode set: +// I/D = 1; Increment by 1 +// S = 0; No shift +// +// Note, however, that resetting the Arduino doesn't reset the LCD, so we +// can't assume that its in that state when a sketch starts (and the +// LiquidCrystal constructor is called). + +LiquidCrystal_I2C::LiquidCrystal_I2C(uint8_t lcd_Addr,uint8_t lcd_cols,uint8_t lcd_rows, TwoWire *wire_instance) +{ + _Addr = lcd_Addr; + _cols = lcd_cols; + _rows = lcd_rows; + _backlightval = LCD_NOBACKLIGHT; + _wire_ptr = wire_instance; +} + +void LiquidCrystal_I2C::oled_init(){ + _oled = true; + init_priv(); +} + +void LiquidCrystal_I2C::init(){ + init_priv(); +} + +void LiquidCrystal_I2C::init_priv() +{ + _wire_ptr->begin(); + _displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS; + begin(_cols, _rows); +} + +void LiquidCrystal_I2C::begin(uint8_t cols, uint8_t lines, uint8_t dotsize) { + if (lines > 1) { + _displayfunction |= LCD_2LINE; + } + _numlines = lines; + + // for some 1 line displays you can select a 10 pixel high font + if ((dotsize != 0) && (lines == 1)) { + _displayfunction |= LCD_5x10DOTS; + } + + // SEE PAGE 45/46 FOR INITIALIZATION SPECIFICATION! + // according to datasheet, we need at least 40ms after power rises above 2.7V + // before sending commands. Arduino can turn on way befer 4.5V so we'll wait 50 + delay(50); + + // Now we pull both RS and R/W low to begin commands + expanderWrite(_backlightval); // reset expanderand turn backlight off (Bit 8 =1) + delay(1000); + + //put the LCD into 4 bit mode + // this is according to the hitachi HD44780 datasheet + // figure 24, pg 46 + + // we start in 8bit mode, try to set 4 bit mode + write4bits(0x03 << 4); + delayMicroseconds(4500); // wait min 4.1ms + + // second try + write4bits(0x03 << 4); + delayMicroseconds(4500); // wait min 4.1ms + + // third go! + write4bits(0x03 << 4); + delayMicroseconds(150); + + // finally, set to 4-bit interface + write4bits(0x02 << 4); + + + // set # lines, font size, etc. + command(LCD_FUNCTIONSET | _displayfunction); + + // turn the display on with no cursor or blinking default + _displaycontrol = LCD_DISPLAYON | LCD_CURSOROFF | LCD_BLINKOFF; + display(); + + // clear it off + clear(); + + // Initialize to default text direction (for roman languages) + _displaymode = LCD_ENTRYLEFT | LCD_ENTRYSHIFTDECREMENT; + + // set the entry mode + command(LCD_ENTRYMODESET | _displaymode); + + home(); + +} + +/********** high level commands, for the user! */ +void LiquidCrystal_I2C::clear(){ + command(LCD_CLEARDISPLAY);// clear display, set cursor position to zero + delayMicroseconds(2000); // this command takes a long time! + if (_oled) setCursor(0,0); +} + +void LiquidCrystal_I2C::home(){ + command(LCD_RETURNHOME); // set cursor position to zero + delayMicroseconds(2000); // this command takes a long time! +} + +void LiquidCrystal_I2C::setCursor(uint8_t col, uint8_t row){ + int row_offsets[] = { 0x00, 0x40, 0x14, 0x54 }; + if ( row > _numlines ) { + row = _numlines-1; // we count rows starting w/0 + } + command(LCD_SETDDRAMADDR | (col + row_offsets[row])); +} + +// Turn the display on/off (quickly) +void LiquidCrystal_I2C::noDisplay() { + _displaycontrol &= ~LCD_DISPLAYON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} +void LiquidCrystal_I2C::display() { + _displaycontrol |= LCD_DISPLAYON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} + +// Turns the underline cursor on/off +void LiquidCrystal_I2C::noCursor() { + _displaycontrol &= ~LCD_CURSORON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} +void LiquidCrystal_I2C::cursor() { + _displaycontrol |= LCD_CURSORON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} + +// Turn on and off the blinking cursor +void LiquidCrystal_I2C::noBlink() { + _displaycontrol &= ~LCD_BLINKON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} +void LiquidCrystal_I2C::blink() { + _displaycontrol |= LCD_BLINKON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} + +// These commands scroll the display without changing the RAM +void LiquidCrystal_I2C::scrollDisplayLeft(void) { + command(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVELEFT); +} +void LiquidCrystal_I2C::scrollDisplayRight(void) { + command(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVERIGHT); +} + +// This is for text that flows Left to Right +void LiquidCrystal_I2C::leftToRight(void) { + _displaymode |= LCD_ENTRYLEFT; + command(LCD_ENTRYMODESET | _displaymode); +} + +// This is for text that flows Right to Left +void LiquidCrystal_I2C::rightToLeft(void) { + _displaymode &= ~LCD_ENTRYLEFT; + command(LCD_ENTRYMODESET | _displaymode); +} + +// This will 'right justify' text from the cursor +void LiquidCrystal_I2C::autoscroll(void) { + _displaymode |= LCD_ENTRYSHIFTINCREMENT; + command(LCD_ENTRYMODESET | _displaymode); +} + +// This will 'left justify' text from the cursor +void LiquidCrystal_I2C::noAutoscroll(void) { + _displaymode &= ~LCD_ENTRYSHIFTINCREMENT; + command(LCD_ENTRYMODESET | _displaymode); +} + +// Allows us to fill the first 8 CGRAM locations +// with custom characters +void LiquidCrystal_I2C::createChar(uint8_t location, uint8_t charmap[]) { + location &= 0x7; // we only have 8 locations 0-7 + command(LCD_SETCGRAMADDR | (location << 3)); + for (int i=0; i<8; i++) { + write(charmap[i]); + } +} + +//createChar with PROGMEM input +void LiquidCrystal_I2C::createChar(uint8_t location, const char *charmap) { + location &= 0x7; // we only have 8 locations 0-7 + command(LCD_SETCGRAMADDR | (location << 3)); + for (int i=0; i<8; i++) { + write(pgm_read_byte_near(charmap++)); + } +} + +// Turn the (optional) backlight off/on +void LiquidCrystal_I2C::noBacklight(void) { + _backlightval=LCD_NOBACKLIGHT; + expanderWrite(0); +} + +void LiquidCrystal_I2C::backlight(void) { + _backlightval=LCD_BACKLIGHT; + expanderWrite(0); +} + + + +/*********** mid level commands, for sending data/cmds */ + +inline void LiquidCrystal_I2C::command(uint8_t value) { + send(value, 0); +} + + +/************ low level data pushing commands **********/ + +// write either command or data +void LiquidCrystal_I2C::send(uint8_t value, uint8_t mode) { + uint8_t highnib=value&0xf0; + uint8_t lownib=(value<<4)&0xf0; + write4bits((highnib)|mode); + write4bits((lownib)|mode); +} + +void LiquidCrystal_I2C::write4bits(uint8_t value) { + expanderWrite(value); + pulseEnable(value); +} + +void LiquidCrystal_I2C::expanderWrite(uint8_t _data){ + _wire_ptr->beginTransmission(_Addr); + printIIC((int)(_data) | _backlightval); + _wire_ptr->endTransmission(); +} + +void LiquidCrystal_I2C::pulseEnable(uint8_t _data){ + expanderWrite(_data | En); // En high + delayMicroseconds(1); // enable pulse must be >450ns + + expanderWrite(_data & ~En); // En low + delayMicroseconds(50); // commands need > 37us to settle +} + + +// Alias functions + +void LiquidCrystal_I2C::cursor_on(){ + cursor(); +} + +void LiquidCrystal_I2C::cursor_off(){ + noCursor(); +} + +void LiquidCrystal_I2C::blink_on(){ + blink(); +} + +void LiquidCrystal_I2C::blink_off(){ + noBlink(); +} + +void LiquidCrystal_I2C::load_custom_character(uint8_t char_num, uint8_t *rows){ + createChar(char_num, rows); +} + +void LiquidCrystal_I2C::setBacklight(uint8_t new_val){ + if(new_val){ + backlight(); // turn backlight on + }else{ + noBacklight(); // turn backlight off + } +} + +void LiquidCrystal_I2C::printstr(const char c[]){ + //This function is not identical to the function used for "real" I2C displays + //it's here so the user sketch doesn't have to be changed + print(c); +} + + +// unsupported API functions +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" +void LiquidCrystal_I2C::off(){} +void LiquidCrystal_I2C::on(){} +void LiquidCrystal_I2C::setDelay (int cmdDelay,int charDelay) {} +uint8_t LiquidCrystal_I2C::status(){return 0;} +uint8_t LiquidCrystal_I2C::keypad (){return 0;} +uint8_t LiquidCrystal_I2C::init_bargraph(uint8_t graphtype){return 0;} +void LiquidCrystal_I2C::draw_horizontal_graph(uint8_t row, uint8_t column, uint8_t len, uint8_t pixel_col_end){} +void LiquidCrystal_I2C::draw_vertical_graph(uint8_t row, uint8_t column, uint8_t len, uint8_t pixel_row_end){} +void LiquidCrystal_I2C::setContrast(uint8_t new_val){} +#pragma GCC diagnostic pop + diff --git a/h4/LiquidCrystal_I2C_pt.h b/h4/LiquidCrystal_I2C_pt.h new file mode 100644 index 0000000..edee63c --- /dev/null +++ b/h4/LiquidCrystal_I2C_pt.h @@ -0,0 +1,137 @@ +//YWROBOT +/* 2024-06-24: + * based on https://github.com/johnrickman/LiquidCrystal_I2C + * added passing of an TwoWire pointer for Controllers with more + * Interfaces and changable pin's + */ +#ifndef LiquidCrystal_I2C_h +#define LiquidCrystal_I2C_h + +#include +#include "Print.h" +#include + +// commands +#define LCD_CLEARDISPLAY 0x01 +#define LCD_RETURNHOME 0x02 +#define LCD_ENTRYMODESET 0x04 +#define LCD_DISPLAYCONTROL 0x08 +#define LCD_CURSORSHIFT 0x10 +#define LCD_FUNCTIONSET 0x20 +#define LCD_SETCGRAMADDR 0x40 +#define LCD_SETDDRAMADDR 0x80 + +// flags for display entry mode +#define LCD_ENTRYRIGHT 0x00 +#define LCD_ENTRYLEFT 0x02 +#define LCD_ENTRYSHIFTINCREMENT 0x01 +#define LCD_ENTRYSHIFTDECREMENT 0x00 + +// flags for display on/off control +#define LCD_DISPLAYON 0x04 +#define LCD_DISPLAYOFF 0x00 +#define LCD_CURSORON 0x02 +#define LCD_CURSOROFF 0x00 +#define LCD_BLINKON 0x01 +#define LCD_BLINKOFF 0x00 + +// flags for display/cursor shift +#define LCD_DISPLAYMOVE 0x08 +#define LCD_CURSORMOVE 0x00 +#define LCD_MOVERIGHT 0x04 +#define LCD_MOVELEFT 0x00 + +// flags for function set +#define LCD_8BITMODE 0x10 +#define LCD_4BITMODE 0x00 +#define LCD_2LINE 0x08 +#define LCD_1LINE 0x00 +#define LCD_5x10DOTS 0x04 +#define LCD_5x8DOTS 0x00 + +// flags for backlight control +#define LCD_BACKLIGHT 0x08 +#define LCD_NOBACKLIGHT 0x00 + +#define En B00000100 // Enable bit +#define Rw B00000010 // Read/Write bit +#define Rs B00000001 // Register select bit + +class LiquidCrystal_I2C : public Print { +public: + LiquidCrystal_I2C(uint8_t lcd_Addr,uint8_t lcd_cols,uint8_t lcd_rows,TwoWire *wire_instance = &Wire); + void begin(uint8_t cols, uint8_t rows, uint8_t charsize = LCD_5x8DOTS ); + void clear(); + void home(); + void noDisplay(); + void display(); + void noBlink(); + void blink(); + void noCursor(); + void cursor(); + void scrollDisplayLeft(); + void scrollDisplayRight(); + void printLeft(); + void printRight(); + void leftToRight(); + void rightToLeft(); + void shiftIncrement(); + void shiftDecrement(); + void noBacklight(); + void backlight(); + void autoscroll(); + void noAutoscroll(); + void createChar(uint8_t, uint8_t[]); + void createChar(uint8_t location, const char *charmap); + // Example: const char bell[8] PROGMEM = {B00100,B01110,B01110,B01110,B11111,B00000,B00100,B00000}; + + void setCursor(uint8_t, uint8_t); +#if defined(ARDUINO) && ARDUINO >= 100 + virtual size_t write(uint8_t); +#else + virtual void write(uint8_t); +#endif + void command(uint8_t); + void init(); + void oled_init(); + +////compatibility API function aliases +void blink_on(); // alias for blink() +void blink_off(); // alias for noBlink() +void cursor_on(); // alias for cursor() +void cursor_off(); // alias for noCursor() +void setBacklight(uint8_t new_val); // alias for backlight() and nobacklight() +void load_custom_character(uint8_t char_num, uint8_t *rows); // alias for createChar() +void printstr(const char[]); + +////Unsupported API functions (not implemented in this library) +uint8_t status(); +void setContrast(uint8_t new_val); +uint8_t keypad(); +void setDelay(int,int); +void on(); +void off(); +uint8_t init_bargraph(uint8_t graphtype); +void draw_horizontal_graph(uint8_t row, uint8_t column, uint8_t len, uint8_t pixel_col_end); +void draw_vertical_graph(uint8_t row, uint8_t column, uint8_t len, uint8_t pixel_col_end); + + +private: + void init_priv(); + void send(uint8_t, uint8_t); + void write4bits(uint8_t); + void expanderWrite(uint8_t); + void pulseEnable(uint8_t); + uint8_t _Addr; + uint8_t _displayfunction; + uint8_t _displaycontrol; + uint8_t _displaymode; + uint8_t _numlines; + bool _oled = false; + uint8_t _cols; + uint8_t _rows; + uint8_t _backlightval; + TwoWire *_wire_ptr = NULL; +}; + +#endif diff --git a/h4/h4.ino b/h4/h4.ino index 4e55609..f538971 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -2,8 +2,10 @@ /* Change values in this section to suit your hardware. */ +#define DISPLAY_I2C_EXPANDER /* Define this for using an Display with I2C expander board */ + // Define your hardware parameters here. -const int ENCODER_PPR = 600; // 600 step spindle optical rotary encoder. Fractional values not supported. +const int ENCODER_PPR = 2000; // 600 step spindle optical rotary encoder. Fractional values not supported. const int ENCODER_BACKLASH = 3; // Numer of impulses encoder can issue without movement of the spindle // Spindle rotary encoder pins. Swap values if the rotation direction is wrong. @@ -13,18 +15,18 @@ const int ENCODER_BACKLASH = 3; // Numer of impulses encoder can issue without m // Main lead screw (Z) parameters. const long SCREW_Z_DU = 20000; // 2mm lead screw in deci-microns (10^-7 of a meter) const long MOTOR_STEPS_Z = 800; -const long SPEED_START_Z = 2 * MOTOR_STEPS_Z; // Initial speed of a motor, steps / second. -const long ACCELERATION_Z = 30 * MOTOR_STEPS_Z; // Acceleration of a motor, steps / second ^ 2. -const long SPEED_MANUAL_MOVE_Z = 6 * MOTOR_STEPS_Z; // Maximum speed of a motor during manual move, steps / second. -const bool INVERT_Z = false; // change (true/false) if the carriage moves e.g. "left" when you press "right". -const bool NEEDS_REST_Z = false; // Set to false for closed-loop drivers, true for open-loop. +const long SPEED_START_Z = 1 * MOTOR_STEPS_Z; // Initial speed of a motor, steps / second. +const long ACCELERATION_Z = 15 * MOTOR_STEPS_Z; // Acceleration of a motor, steps / second ^ 2. +const long SPEED_MANUAL_MOVE_Z = 3 * MOTOR_STEPS_Z; // Maximum speed of a motor during manual move, steps / second. +const bool INVERT_Z = true; // change (true/false) if the carriage moves e.g. "left" when you press "right". +const bool NEEDS_REST_Z = true; // Set to false for closed-loop drivers, true for open-loop. const long MAX_TRAVEL_MM_Z = 300; // Lathe bed doesn't allow to travel more than this in one go, 30cm / ~1 foot const long BACKLASH_DU_Z = 6500; // 0.65mm backlash in deci-microns (10^-7 of a meter) const char NAME_Z = 'Z'; // Text shown on screen before axis position value, GCode axis name // Cross-slide lead screw (X) parameters. -const long SCREW_X_DU = 12500; // 1.25mm lead screw with 3x reduction in deci-microns (10^-7) of a meter -const long MOTOR_STEPS_X = 2400; // 800 steps at 3x reduction +const long SCREW_X_DU = 20000; // 2mm lead screw with 3x reduction in deci-microns (10^-7) of a meter +const long MOTOR_STEPS_X = 600; // 200 steps at 3x reduction const long SPEED_START_X = MOTOR_STEPS_X; // Initial speed of a motor, steps / second. const long ACCELERATION_X = 10 * MOTOR_STEPS_X; // Acceleration of a motor, steps / second ^ 2. const long SPEED_MANUAL_MOVE_X = 3 * MOTOR_STEPS_X; // Maximum speed of a motor during manual move, steps / second. @@ -67,11 +69,12 @@ const long PULSE_MIN_WIDTH_US = 1000; // Microseconds width of the pulse that is const long PULSE_HALF_BACKLASH = 2; // Prevents spurious reverses when moving using a handwheel. Raise to 3 or 4 if they still happen. const int ENCODER_STEPS_INT = ENCODER_PPR * 2; // Number of encoder impulses PCNT counts per revolution of the spindle -const int ENCODER_FILTER = 2; // Encoder pulses shorter than this will be ignored. Clock cycles, 1 - 1023. +//const int ENCODER_FILTER = 2; // Encoder pulses shorter than this will be ignored. Clock cycles, 1 - 1023. +const int ENCODER_FILTER_NS = 25; // Encoder pulses shorter than this will be ignored. In ns ---> Todo, is this too short? const int PCNT_LIM = 31000; // Limit used in hardware pulse counter logic. const int PCNT_CLEAR = 30000; // Limit where we reset hardware pulse counter value to avoid overflow. Less than PCNT_LIM. const long DUPR_MAX = 254000; // No more than 1 inch pitch -const int32_t STARTS_MAX = 124; // No more than 124-start thread +const int STARTS_MAX = 124; // No more than 124-start thread const long PASSES_MAX = 999; // No more turn or face passes than this const long SAFE_DISTANCE_DU = 5000; // Step back 0.5mm from the material when moving between cuts in automated modes const long SAVE_DELAY_US = 5000000; // Wait 5s after last save and last change of saveable data before saving again @@ -91,22 +94,22 @@ const bool SPINDLE_PAUSES_GCODE = true; // pause GCode execution when spindle st const int GCODE_MIN_RPM = 30; // pause GCode execution if RPM is below this // To be incremented whenever a measurable improvement is made. -#define SOFTWARE_VERSION 12 +#define SOFTWARE_VERSION 99 // To be changed whenever a different PCB / encoder / stepper / ... design is used. -#define HARDWARE_VERSION 5 +#define HARDWARE_VERSION 4 #define Z_ENA 16 #define Z_DIR 17 #define Z_STEP 18 #define X_ENA 8 -#define X_DIR 19 -#define X_STEP 20 +#define X_DIR 21 +#define X_STEP 48 #define BUZZ 4 -#define SCL 5 -#define SDA 6 +#define KEYS_SCL 5 +#define KEYS_SDA 6 #define A11 9 #define A12 10 @@ -115,34 +118,43 @@ const int GCODE_MIN_RPM = 30; // pause GCode execution if RPM is below this #define A21 12 #define A22 13 #define A23 14 -//code keypad -/* - C0 C1 C2 C3 C4 C5 C6 -R0 1 2 3 4 5 6 7 -R1 11 12 13 14 15 16 17 -R2 21 22 23 24 25 26 27 -R3 31 32 33 34 35 36 37 -R4 41 42 43 44 45 46 47 -R5 51 52 53 54 55 56 57 -R6 61 62 63 64 65 66 67 -*/ + +#ifdef DISPLAY_I2C_EXPANDER +// If using display with I2C expander, only two pins are used +#define DISP_SDA 2 +#define DISP_SCL 1 +#else +// Default standard display +#define DISP_RS 21 +#define DISP_EN 48 +#define DISP_D0 47 +#define DISP_D1 38 +#define DISP_D2 39 +#define DISP_D3 40 +#define DISP_D4 41 +#define DISP_D5 42 +#define DISP_D6 2 +#define DISP_D7 1 +#endif + + #define B_LEFT 57 #define B_RIGHT 37 #define B_UP 47 #define B_DOWN 67 #define B_MINUS 5 #define B_PLUS 64 -#define B_ON 17 //START -#define B_OFF 27 //STOP -#define B_STOPL 7 //LLEFT -#define B_STOPR 15 //LRIGHT -#define B_STOPU 6 //LUP -#define B_STOPD 16 //LDOWN -#define B_DISPL 14 //M5 -#define B_STEP 24 //M4 -#define B_SETTINGS 34 //M3 -#define B_MEASURE 54 //M1 -#define B_REVERSE 44 //M2 +#define B_ON 17 +#define B_OFF 27 +#define B_STOPL 7 +#define B_STOPR 15 +#define B_STOPU 6 +#define B_STOPD 16 +#define B_DISPL 14 +#define B_STEP 24 +#define B_SETTINGS 34 +#define B_MEASURE 54 +#define B_REVERSE 44 #define B_0 51 #define B_1 41 #define B_2 61 @@ -153,18 +165,18 @@ R6 61 62 63 64 65 66 67 #define B_7 11 #define B_8 22 #define B_9 1 -#define B_BACKSPACE 32 //BS -#define B_MODE_GEARS 42 //F1 -#define B_MODE_TURN 52 //F2 -#define B_MODE_FACE 62 //F3 -#define B_MODE_CONE 3 //F4 -#define B_MODE_CUT 13 //F5 -#define B_MODE_THREAD 23 //F6 -#define B_MODE_OTHER 33 //F7 -#define B_X 53 //M8 -#define B_Z 43 //M9 -#define B_A 4 //M6 -#define B_B 63 //M7 +#define B_BACKSPACE 32 +#define B_MODE_GEARS 42 +#define B_MODE_TURN 52 +#define B_MODE_FACE 62 +#define B_MODE_CONE 3 +#define B_MODE_CUT 13 +#define B_MODE_THREAD 23 +#define B_MODE_OTHER 33 +#define B_X 53 +#define B_Z 43 +#define B_A 4 +#define B_B 63 #define PREF_VERSION "v" #define PREF_DUPR "d" @@ -252,12 +264,24 @@ const float GCODE_FEED_MIN_DU_SEC = 167; // Minimum feed in du/sec in GCode mode #define DELAY(x) vTaskDelay(x / portTICK_PERIOD_MS); // ESP32 hardware pulse counter library used to count spindle encoder pulses. -#include "driver/pcnt.h" +//#include "driver/pcnt.h" +#include "driver/pulse_cnt.h" #include #include -#include -LiquidCrystal_I2C lcd(0x27, 20, 4); + +TwoWire I2C_keys = TwoWire(0); + +#ifdef DISPLAY_I2C_EXPANDER + TwoWire I2C_disp = TwoWire(1); // Generate a secondary I2C object + #include "LiquidCrystal_I2C_pt.h" + //I2C_disp.setPins(DISP_SDA, DISP_SCL); + LiquidCrystal_I2C lcd(0x27,20,4, &I2C_disp); +#else + #include + LiquidCrystal lcd(DISP_RS, DISP_EN, DISP_D0, DISP_D1, DISP_D2, DISP_D3, DISP_D4, DISP_D5, DISP_D6, DISP_D7); +#endif + #define LCD_HASH_INITIAL -3845709 // Random number that's unlikely to naturally occur as an actual hash long lcdHashLine0 = LCD_HASH_INITIAL; long lcdHashLine1 = LCD_HASH_INITIAL; @@ -362,11 +386,11 @@ struct Axis { int step; // Step pin of this motor }; +pcnt_unit_handle_t pcnt_unit_1 = NULL; +pcnt_channel_handle_t pcnt_chan_1A = NULL; + void initAxis(Axis* a, char name, bool active, bool rotational, float motorSteps, float screwPitch, long speedStart, long speedManualMove, long acceleration, bool invertStepper, bool needsRest, long maxTravelMm, long backlashDu, int ena, int dir, int step) { - Wire.begin(SDA, SCL); - lcd.init(); // initialize the lcd - lcd.backlight(); a->mutex = xSemaphoreCreateMutex(); a->name = name; @@ -580,7 +604,9 @@ int gcodeProgramCount = 0; String gcodeProgram = ""; int gcodeProgramCharIndex = 0; -hw_timer_t *async_timer = timerBegin(80); +#define TIMER_RESOLUTION_HZ 1000000UL /* 1MHz resolution */ +// https://docs.espressif.com/projects/arduino-esp32/en/latest/api/timer.html +hw_timer_t *async_timer = NULL; bool timerAttached = false; int getApproxRpm() { @@ -1050,13 +1076,6 @@ void IRAM_ATTR pulse2Enc() { } } -void setAsyncTimerEnable(bool value) { - if (value) { - timerStart(async_timer); - } else { - timerStop(async_timer); - } -} void taskDisplay(void *param) { while (emergencyStop == ESTOP_NONE) { @@ -1592,29 +1611,51 @@ bool removeAllGcode() { return true; } -void startPulseCounter(pcnt_unit_t unit, int gpioA, int gpioB) { - pcnt_config_t pcntConfig; - pcntConfig.pulse_gpio_num = gpioA; - pcntConfig.ctrl_gpio_num = gpioB; - pcntConfig.channel = PCNT_CHANNEL_0; - pcntConfig.unit = unit; - pcntConfig.pos_mode = PCNT_COUNT_INC; - pcntConfig.neg_mode = PCNT_COUNT_DEC; - pcntConfig.lctrl_mode = PCNT_MODE_REVERSE; - pcntConfig.hctrl_mode = PCNT_MODE_KEEP; - pcntConfig.counter_h_lim = PCNT_LIM; - pcntConfig.counter_l_lim = -PCNT_LIM; - pcnt_unit_config(&pcntConfig); - pcnt_set_filter_value(unit, ENCODER_FILTER); - pcnt_filter_enable(unit); - pcnt_counter_pause(unit); - pcnt_counter_clear(unit); - pcnt_counter_resume(unit); +/* Init and start of pulse counter for encoder interface */ +/* References + * https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/pcnt.html + * https://github.com/espressif/esp-idf/blob/v5.2.2/examples/peripherals/pcnt/rotary_encoder/main/rotary_encoder_example_main.c + */ +void startPulseCounter() +{ + pcnt_unit_config_t pcnt_unit_config; + pcnt_chan_config_t pcnt_chan_config; + pcnt_glitch_filter_config_t filter_config; + + Serial.println("Start PCNT encoder setup..."); + + // install pcnt unit + pcnt_unit_config.low_limit = -PCNT_LIM; + pcnt_unit_config.high_limit = PCNT_LIM; + ESP_ERROR_CHECK(pcnt_new_unit(&pcnt_unit_config, &pcnt_unit_1)); + + // set glitch filter + filter_config.max_glitch_ns = ENCODER_FILTER_NS; + ESP_ERROR_CHECK(pcnt_unit_set_glitch_filter(pcnt_unit_1, &filter_config)); + + // install pcnt channel + pcnt_chan_config.edge_gpio_num = ENC_A; + pcnt_chan_config.level_gpio_num = ENC_B; + ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit_1, &pcnt_chan_config, &pcnt_chan_1A)); + + // set edge and level actions for pcnt channels + // .... channel, POS or HCTRL , NEG or LCTRL + ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_1A, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE)); + ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_1A, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE)); + + // enable pcnt unit without counting + ESP_ERROR_CHECK(pcnt_unit_enable(pcnt_unit_1)); + + // Clear PCNT pulse count value to zero. + ESP_ERROR_CHECK(pcnt_unit_clear_count(pcnt_unit_1)); + + // Start the PCNT unit, the counter will start to count according to the edge and/or level input signals. + ESP_ERROR_CHECK(pcnt_unit_start(pcnt_unit_1)); } // Attaching interrupt on core 0 to have more time on core 1 where axes are moved. void taskAttachInterrupts(void *param) { - startPulseCounter(PCNT_UNIT_0, ENC_A, ENC_B); + startPulseCounter(); if (PULSE_1_USE) attachInterrupt(digitalPinToInterrupt(A12), pulse1Enc, CHANGE); if (PULSE_2_USE) attachInterrupt(digitalPinToInterrupt(A22), pulse2Enc, CHANGE); vTaskDelete(NULL); @@ -1622,13 +1663,16 @@ void taskAttachInterrupts(void *param) { void setEmergencyStop(int kind) { emergencyStop = kind; - setAsyncTimerEnable(false); + timerStop(async_timer); xSemaphoreTake(z.mutex, 10); xSemaphoreTake(x.mutex, 10); xSemaphoreTake(a1.mutex, 10); } void setup() { + Serial.begin(115200); + Serial.println("Start setup..."); + pinMode(ENC_A, INPUT_PULLUP); pinMode(ENC_B, INPUT_PULLUP); @@ -1665,6 +1709,9 @@ void setup() { DLOW(A21); } + // Starting the hardware timer + async_timer = timerBegin(TIMER_RESOLUTION_HZ); + Preferences pref; pref.begin(PREF_NAMESPACE); if (pref.getInt(PREF_VERSION) != PREFERENCES_VERSION) { @@ -1679,7 +1726,7 @@ void setup() { isOn = false; savedDupr = dupr = pref.getLong(PREF_DUPR); motionMutex = xSemaphoreCreateMutex(); - savedStarts = starts = min(STARTS_MAX, max(static_cast(1), pref.getInt(PREF_STARTS))); + savedStarts = starts = min((int32_t)STARTS_MAX, max((int32_t)1, pref.getInt(PREF_STARTS))); z.savedPos = z.pos = pref.getLong(PREF_POS_Z); z.savedPosGlobal = z.posGlobal = pref.getLong(PREF_POS_GLOBAL_Z); z.savedOriginPos = z.originPos = pref.getLong(PREF_ORIGIN_POS_Z); @@ -1736,7 +1783,15 @@ void setup() { } pref.end(); - lcd.begin(20, 4); + Serial.println("Init display..."); + #ifdef DISPLAY_I2C_EXPANDER + I2C_disp.setPins(DISP_SDA, DISP_SCL); + lcd.init(); + lcd.backlight(); + #else + lcd.begin(20, 4); + #endif + lcd.createChar(customCharMmCode, customCharMm); lcd.createChar(customCharLimLeftCode, customCharLimLeft); lcd.createChar(customCharLimRightCode, customCharLimRight); @@ -1745,11 +1800,11 @@ void setup() { lcd.createChar(customCharLimUpDownCode, customCharLimUpDown); lcd.createChar(customCharLimLeftRightCode, customCharLimLeftRight); - Serial.begin(115200); - if (!Wire.begin(SDA, SCL)) { + + if (!I2C_keys.begin(KEYS_SDA, KEYS_SCL)) { Serial.println("I2C initialization failed"); - } else if (!keypad.begin(TCA8418_DEFAULT_ADDR, &Wire)) { + } else if (!keypad.begin(TCA8418_DEFAULT_ADDR, &I2C_keys)) { Serial.println("TCA8418 key controller not found"); } else { keypad.matrix(7, 7); @@ -1874,10 +1929,10 @@ void updateAsyncTimerSettings() { // dupr and therefore direction can change while we're in async mode. setDir(getAsyncAxis(), dupr > 0); - // dupr can change while we're in async mode, keep updating timer frequency. - timerAlarm(async_timer, getTimerLimit(), true, 0); - // without this timer stops working if already above new limit + timerStop(async_timer); timerWrite(async_timer, 0); + // dupr can change while we're in async mode, keep updating timer frequency. + timerAlarm(async_timer, getTimerLimit(), true, 0); /* Automatically enables Alarm*/ } void setDupr(long value) { @@ -1931,6 +1986,7 @@ unsigned int getTimerLimit() { // Only used for async movement in ASYNC and A1 modes. // Keep code in this method to absolute minimum to achieve high stepper speeds. void IRAM_ATTR onAsyncTimer() { + //Serial.print("A"); //for debug -throws wdt reset!!! Axis* a = getAsyncAxis(); if (!isOn || a->movingManually || (mode != MODE_ASYNC && mode != MODE_A1)) { return; @@ -1971,7 +2027,7 @@ void setModeFromLoop(int value) { if (mode == MODE_THREAD) { setStarts(1); } else if (mode == MODE_ASYNC || mode == MODE_A1) { - setAsyncTimerEnable(false); + timerStop(async_timer); } mode = value; setupIndex = 0; @@ -1981,7 +2037,7 @@ void setModeFromLoop(int value) { timerAttachInterrupt(async_timer, &onAsyncTimer); } updateAsyncTimerSettings(); - setAsyncTimerEnable(true); + timerStart(async_timer); } } @@ -3230,52 +3286,57 @@ void discountFullSpindleTurns() { } } -void processSpindleCounter() { - int16_t count; - pcnt_get_counter_value(PCNT_UNIT_0, &count); - int delta = count - spindleCount; - if (delta == 0) { - return; - } - if (count >= PCNT_CLEAR || count <= -PCNT_CLEAR) { - pcnt_counter_clear(PCNT_UNIT_0); - spindleCount = 0; - } else { - spindleCount = count; - } +void processSpindleCounter() +{ + if(pcnt_unit_1 != NULL) + { + // Checking spindle counter only after pcnt_unit_1 is ready + int count; + pcnt_unit_get_count(pcnt_unit_1, &count); + int delta = count - spindleCount; + if (delta == 0) { + return; + } + if (count >= PCNT_CLEAR || count <= -PCNT_CLEAR) { + pcnt_unit_clear_count(pcnt_unit_1); + spindleCount = 0; + } else { + spindleCount = count; + } - unsigned long microsNow = micros(); - if (showTacho || mode == MODE_GCODE) { - if (spindleEncTimeIndex >= RPM_BULK) { - spindleEncTimeDiffBulk = microsNow - spindleEncTimeAtIndex0; - spindleEncTimeAtIndex0 = microsNow; - spindleEncTimeIndex = 0; + unsigned long microsNow = micros(); + if (showTacho || mode == MODE_GCODE) { + if (spindleEncTimeIndex >= RPM_BULK) { + spindleEncTimeDiffBulk = microsNow - spindleEncTimeAtIndex0; + spindleEncTimeAtIndex0 = microsNow; + spindleEncTimeIndex = 0; + } + spindleEncTimeIndex += abs(delta); + } else { + spindleEncTimeDiffBulk = 0; } - spindleEncTimeIndex += abs(delta); - } else { - spindleEncTimeDiffBulk = 0; - } - spindlePos += delta; - spindlePosGlobal += delta; - if (spindlePosGlobal > ENCODER_STEPS_INT) { - spindlePosGlobal -= ENCODER_STEPS_INT; - } else if (spindlePosGlobal < 0) { - spindlePosGlobal += ENCODER_STEPS_INT; - } - if (spindlePos > spindlePosAvg) { - spindlePosAvg = spindlePos; - } else if (spindlePos < spindlePosAvg - ENCODER_BACKLASH) { - spindlePosAvg = spindlePos + ENCODER_BACKLASH; - } - spindleEncTime = microsNow; + spindlePos += delta; + spindlePosGlobal += delta; + if (spindlePosGlobal > ENCODER_STEPS_INT) { + spindlePosGlobal -= ENCODER_STEPS_INT; + } else if (spindlePosGlobal < 0) { + spindlePosGlobal += ENCODER_STEPS_INT; + } + if (spindlePos > spindlePosAvg) { + spindlePosAvg = spindlePos; + } else if (spindlePos < spindlePosAvg - ENCODER_BACKLASH) { + spindlePosAvg = spindlePos + ENCODER_BACKLASH; + } + spindleEncTime = microsNow; - if (spindlePosSync != 0) { - spindlePosSync += delta; - if (spindlePosSync % ENCODER_STEPS_INT == 0) { - spindlePosSync = 0; - Axis* a = getPitchAxis(); - spindlePosAvg = spindlePos = spindleFromPos(a, a->pos); + if (spindlePosSync != 0) { + spindlePosSync += delta; + if (spindlePosSync % ENCODER_STEPS_INT == 0) { + spindlePosSync = 0; + Axis* a = getPitchAxis(); + spindlePosAvg = spindlePos = spindleFromPos(a, a->pos); + } } } } @@ -3359,4 +3420,4 @@ void loop() { moveAxis(&x); if (ACTIVE_A1) moveAxis(&a1); xSemaphoreGive(motionMutex); -} +} \ No newline at end of file From 5c98619bd891f996b3d3808d8497486290545cd3 Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Tue, 5 Nov 2024 22:36:37 +0300 Subject: [PATCH 13/14] Update h4.ino --- h4/h4.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/h4/h4.ino b/h4/h4.ino index f538971..3bb8dff 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -104,8 +104,8 @@ const int GCODE_MIN_RPM = 30; // pause GCode execution if RPM is below this #define Z_STEP 18 #define X_ENA 8 -#define X_DIR 21 -#define X_STEP 48 +#define X_DIR 45 +#define X_STEP 19 #define BUZZ 4 #define KEYS_SCL 5 From 2777a047ae92a982ba9e5d0e4a1e502ec1b82961 Mon Sep 17 00:00:00 2001 From: Kotovasia Date: Sun, 22 Dec 2024 23:33:09 +0300 Subject: [PATCH 14/14] Update h4.ino --- h4/h4.ino | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/h4/h4.ino b/h4/h4.ino index 3bb8dff..4766b0c 100644 --- a/h4/h4.ino +++ b/h4/h4.ino @@ -15,17 +15,17 @@ const int ENCODER_BACKLASH = 3; // Numer of impulses encoder can issue without m // Main lead screw (Z) parameters. const long SCREW_Z_DU = 20000; // 2mm lead screw in deci-microns (10^-7 of a meter) const long MOTOR_STEPS_Z = 800; -const long SPEED_START_Z = 1 * MOTOR_STEPS_Z; // Initial speed of a motor, steps / second. -const long ACCELERATION_Z = 15 * MOTOR_STEPS_Z; // Acceleration of a motor, steps / second ^ 2. -const long SPEED_MANUAL_MOVE_Z = 3 * MOTOR_STEPS_Z; // Maximum speed of a motor during manual move, steps / second. +const long SPEED_START_Z = 0.1 * MOTOR_STEPS_Z; // Initial speed of a motor, steps / second. +const long ACCELERATION_Z = 20 * MOTOR_STEPS_Z; // Acceleration of a motor, steps / second ^ 2. +const long SPEED_MANUAL_MOVE_Z = 12 * MOTOR_STEPS_Z; // Maximum speed of a motor during manual move, steps / second. const bool INVERT_Z = true; // change (true/false) if the carriage moves e.g. "left" when you press "right". const bool NEEDS_REST_Z = true; // Set to false for closed-loop drivers, true for open-loop. const long MAX_TRAVEL_MM_Z = 300; // Lathe bed doesn't allow to travel more than this in one go, 30cm / ~1 foot -const long BACKLASH_DU_Z = 6500; // 0.65mm backlash in deci-microns (10^-7 of a meter) +const long BACKLASH_DU_Z = 4500; // 0.65mm backlash in deci-microns (10^-7 of a meter) const char NAME_Z = 'Z'; // Text shown on screen before axis position value, GCode axis name // Cross-slide lead screw (X) parameters. -const long SCREW_X_DU = 20000; // 2mm lead screw with 3x reduction in deci-microns (10^-7) of a meter +const long SCREW_X_DU = 10000; // 2mm lead screw with 3x reduction in deci-microns (10^-7) of a meter const long MOTOR_STEPS_X = 600; // 200 steps at 3x reduction const long SPEED_START_X = MOTOR_STEPS_X; // Initial speed of a motor, steps / second. const long ACCELERATION_X = 10 * MOTOR_STEPS_X; // Acceleration of a motor, steps / second ^ 2. @@ -33,7 +33,7 @@ const long SPEED_MANUAL_MOVE_X = 3 * MOTOR_STEPS_X; // Maximum speed of a motor const bool INVERT_X = true; // change (true/false) if the carriage moves e.g. "left" when you press "right". const bool NEEDS_REST_X = false; // Set to false for all kinds of drivers or X will be unlocked when not moving. const long MAX_TRAVEL_MM_X = 100; // Cross slide doesn't allow to travel more than this in one go, 10cm -const long BACKLASH_DU_X = 1500; // 0.15mm backlash in deci-microns (10^-7 of a meter) +const long BACKLASH_DU_X = 4700; // 0.15mm backlash in deci-microns (10^-7 of a meter) const char NAME_X = 'X'; // Text shown on screen before axis position value, GCode axis name // Manual stepping with left/right/up/down buttons. Only used when step isn't default continuous (1mm or 0.1"). @@ -70,7 +70,7 @@ const long PULSE_HALF_BACKLASH = 2; // Prevents spurious reverses when moving us const int ENCODER_STEPS_INT = ENCODER_PPR * 2; // Number of encoder impulses PCNT counts per revolution of the spindle //const int ENCODER_FILTER = 2; // Encoder pulses shorter than this will be ignored. Clock cycles, 1 - 1023. -const int ENCODER_FILTER_NS = 25; // Encoder pulses shorter than this will be ignored. In ns ---> Todo, is this too short? +const int ENCODER_FILTER_NS = 60; // Encoder pulses shorter than this will be ignored. In ns ---> Todo, is this too short? const int PCNT_LIM = 31000; // Limit used in hardware pulse counter logic. const int PCNT_CLEAR = 30000; // Limit where we reset hardware pulse counter value to avoid overflow. Less than PCNT_LIM. const long DUPR_MAX = 254000; // No more than 1 inch pitch @@ -104,7 +104,7 @@ const int GCODE_MIN_RPM = 30; // pause GCode execution if RPM is below this #define Z_STEP 18 #define X_ENA 8 -#define X_DIR 45 +#define X_DIR 20 #define X_STEP 19 #define BUZZ 4