From 086b8175cbd86e066beb7431955c83dc8d5204d3 Mon Sep 17 00:00:00 2001 From: atlesg Date: Tue, 21 Dec 2021 16:07:42 +0700 Subject: [PATCH 01/10] First steps for my own customized board - Add my_machine_map.h - Add f4mpv5_hosoi.c - Add LED blinking plugin --- .cproject | 40 ++-- .gitignore | 2 + .settings/language.settings.xml | 36 +-- .vscode/extensions.json | 7 + .vscode/launch.json | 47 ++++ Inc/my_machine.h | 6 +- Inc/my_machine_map.h | 131 +++++++++++ STM32F4xx Debug.launch | 79 +++++++ Src/f4mpv5_hosoi.c | 376 ++++++++++++++++++++++++++++++++ 9 files changed, 683 insertions(+), 41 deletions(-) create mode 100644 .vscode/extensions.json create mode 100644 .vscode/launch.json create mode 100644 Inc/my_machine_map.h create mode 100644 STM32F4xx Debug.launch create mode 100644 Src/f4mpv5_hosoi.c diff --git a/.cproject b/.cproject index a6a9f275..b7d7af32 100644 --- a/.cproject +++ b/.cproject @@ -46,7 +46,6 @@ @@ -2521,64 +2521,64 @@ - + - + - + - + - + - - + + - + - + - - - - - + + - + - + - + - + - + + + + diff --git a/.gitignore b/.gitignore index bd486977..1018b798 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,5 @@ build .vscode/settings.json .vscode/c_cpp_properties.json + +Debug diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index d263907b..1e4afb2e 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -6,7 +6,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -30,7 +30,7 @@ - + @@ -42,7 +42,7 @@ - + @@ -54,7 +54,7 @@ - + @@ -66,7 +66,7 @@ - + @@ -78,7 +78,7 @@ - + @@ -90,7 +90,7 @@ - + @@ -102,7 +102,7 @@ - + @@ -114,7 +114,7 @@ - + @@ -126,7 +126,7 @@ - + @@ -138,7 +138,7 @@ - + @@ -150,7 +150,7 @@ - + @@ -162,7 +162,7 @@ - + @@ -174,7 +174,7 @@ - + @@ -186,7 +186,7 @@ - + @@ -198,7 +198,7 @@ - + @@ -210,7 +210,7 @@ - + diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 00000000..e80666bf --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 00000000..45ae99f6 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,47 @@ +// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY +// +// PIO Unified Debugger +// +// Documentation: https://docs.platformio.org/page/plus/debugging.html +// Configuration: https://docs.platformio.org/page/projectconf/section_env_debug.html + +{ + "version": "0.2.0", + "configurations": [ + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug", + "executable": "e:/TAI/01_AKB/STM32CubeIDE/STM32F4xx/.pio/build/btt_skr_pro_1_1/firmware.elf", + "projectEnvName": "btt_skr_pro_1_1", + "toolchainBinDir": "C:/Users/tai/.platformio/packages/toolchain-gccarmnoneeabi/bin", + "internalConsoleOptions": "openOnSessionStart", + "svdPath": "C:/Users/tai/.platformio/platforms/ststm32/misc/svd/STM32F40x.svd", + "preLaunchTask": { + "type": "PlatformIO", + "task": "Pre-Debug" + } + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (skip Pre-Debug)", + "executable": "e:/TAI/01_AKB/STM32CubeIDE/STM32F4xx/.pio/build/btt_skr_pro_1_1/firmware.elf", + "projectEnvName": "btt_skr_pro_1_1", + "toolchainBinDir": "C:/Users/tai/.platformio/packages/toolchain-gccarmnoneeabi/bin", + "internalConsoleOptions": "openOnSessionStart", + "svdPath": "C:/Users/tai/.platformio/platforms/ststm32/misc/svd/STM32F40x.svd" + }, + { + "type": "platformio-debug", + "request": "launch", + "name": "PIO Debug (without uploading)", + "executable": "e:/TAI/01_AKB/STM32CubeIDE/STM32F4xx/.pio/build/btt_skr_pro_1_1/firmware.elf", + "projectEnvName": "btt_skr_pro_1_1", + "toolchainBinDir": "C:/Users/tai/.platformio/packages/toolchain-gccarmnoneeabi/bin", + "internalConsoleOptions": "openOnSessionStart", + "svdPath": "C:/Users/tai/.platformio/platforms/ststm32/misc/svd/STM32F40x.svd", + "loadMode": "manual" + } + ] +} diff --git a/Inc/my_machine.h b/Inc/my_machine.h index 7adb5acd..dd5103a6 100644 --- a/Inc/my_machine.h +++ b/Inc/my_machine.h @@ -32,7 +32,7 @@ //#define BOARD_BTT_SKR_PRO_1_1 // F407 based 3D Printer board //#define BOARD_BTT_SKR_PRO_1_2 // F407 based 3D Printer board //#define BOARD_BTT_SKR_20 // F407 based 3D Printer board -//#define BOARD_MY_MACHINE // Add my_machine_map.h before enabling this! +#define BOARD_MY_MACHINE // Add my_machine_map.h before enabling this! // WARNING: BOARD_BTT_SKR_20 may fry your Trinamic drivers due to bad hardware design. // The risk goes away if Q1 (HY1904C2) is shorted between source (S) and drain (D). @@ -52,9 +52,9 @@ #define USB_SERIAL_CDC 1 // Serial communication via native USB. #endif //#define BLUETOOTH_ENABLE 1 // Set to 1 for HC-05 module. Requires and claims one auxillary input pin. -//#define HUANYANG_ENABLE 1 // Set to 1 or 2 for Huanyang VFD spindle. +#define VFD_ENABLE 99 // Set to 1 or 2 for Huanyang VFD spindle. More here https://github.com/grblHAL/Plugins_spindle //#define DUAL_SPINDLE 1 // Uncomment for switching between VFD spindle and PWM output with $32 -//#define MODBUS_ENABLE 1 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin. +#define MODBUS_ENABLE 1 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin. //#define SDCARD_ENABLE 2 // Run gcode programs from SD card. //#define KEYPAD_ENABLE 1 // Set to 1 for I2C keypad, 2 for other input such as serial data //#define ODOMETER_ENABLE 1 // Odometer plugin. diff --git a/Inc/my_machine_map.h b/Inc/my_machine_map.h new file mode 100644 index 00000000..d3605c7a --- /dev/null +++ b/Inc/my_machine_map.h @@ -0,0 +1,131 @@ +/* + my_machine_map.h - driver code for F4MPV5 (STM32F407) board + + Part of GrblHAL + + Copyright (c) 2021 atlesg + + Grbl 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. + + Grbl 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 Grbl. If not, see . +*/ + +/* Default Pin Assignments: + * A0 X Step | B0 Step En/Dis | C0 (N/A) + * A1 X Direction | B1 Spindle Enable | C1 (N/A) + * A2 Y Step | B2 Spindle Direction | C2 (N/A) + * A3 Y Direction | B3 | C3 (N/A) + * A4 Z Step | B4 | C4 (N/A) + * A5 Z Direction | B5 | C5 (N/A) + * A6 A Step | B6 Reset | C6 (N/A) + * A7 A Direction | B7 Feed Hold | C7 (N/A) + * A8 Spindle PWM | B8 Cycle Start | C8 (N/A) + * A9 Y2 Step | B9 Door Safety | C9 (N/A) + * A10 Y2 Direction | B10 Y2 Limit | C10 (N/A) + * A11 | B11 | C11 (N/A) + * A12 | B12 X Limit | C12 (N/A) + * A13 | B13 Y Limit | C13 + * A14 | B14 Z Limit | C14 Coolant Flood + * A15 | B15 Probe | C15 coolant Mist + */ + + + +#if N_ABC_MOTORS > 1 +#error "Axis configuration is not supported!" +#endif + +#if !defined(STM32F407xx) || HSE_VALUE != 8000000 +#error "This board has STM32F407 processor with a 8MHz crystal, select a corresponding build!" +#endif + +#define BOARD_NAME "F4MPV5_HOSOI" +#define HAS_BOARD_INIT + + + +// Define step pulse output pins. +#define STEP_PORT GPIOA +#define X_STEP_PIN 2 // JM1 +#define Y_STEP_PIN 4 // JM2 +#define Z_STEP_PIN 6 // JM3 +#define STEP_OUTMODE GPIO_MAP + + +#define DIRECTION_PORT GPIOA +#define X_DIRECTION_PIN 3 // JM1 +#define Y_DIRECTION_PIN 5 // JM2 +#define Z_DIRECTION_PIN 7 // JM3 +#define DIRECTION_OUTMODE GPIO_MAP + +// Define stepper driver enable/disable output pin. +#define STEPPERS_ENABLE_PORT GPIOA // JM4-PU +#define STEPPERS_ENABLE_PIN 6 +#define STEPPERS_ENABLE_MASK STEPPERS_ENABLE_BIT + +// Define homing/hard limit switch input pins. +#define LIMIT_PORT GPIOD +#define X_LIMIT_PIN 13 // N12 +#define Y_LIMIT_PIN 14 // N13 +#define Z_LIMIT_PIN 15 // N14 +#define LIMIT_INMODE GPIO_SHIFT12 + +// Define ganged axis or A axis step pulse and step direction output pins. +#if N_ABC_MOTORS == 1 +#define M3_AVAILABLE +#define M3_STEP_PORT STEP_PORT +#define M3_STEP_PIN 6 +#define M3_DIRECTION_PORT DIRECTION_PORT +#define M3_DIRECTION_PIN 7 +#if N_AUTO_SQUARED +#define M3_LIMIT_PORT LIMIT_PORT +#define M3_LIMIT_PIN 15 +#endif +#endif + + // Define spindle enable and spindle direction output pins. +//#define SPINDLE_ENABLE_PORT GPIOB +//#define SPINDLE_ENABLE_PIN 0 // JM5 - PU +//#define SPINDLE_DIRECTION_PORT GPIOB +//#define SPINDLE_DIRECTION_PIN 1 // JM5 - DR + +#define SPINDLE_CW_PORT GPIOE +#define SPINDLE_CW_PIN 2 +#define SPINDLE_CCW_PORT GPIOE +#define SPINDLE_CCW_PIN 3 + +// Define spindle PWM output pin. +//#define SPINDLE_PWM_PORT_BASE GPIOA_BASE +//#define SPINDLE_PWM_PIN 8 + +// Define flood and mist coolant enable output pins. +#define COOLANT_FLOOD_PORT GPIOE +#define COOLANT_FLOOD_PIN 1 +#define COOLANT_MIST_PORT GPIOB +#define COOLANT_MIST_PIN 4 + +// Define user-control controls (cycle start, reset, feed hold) input pins. +#define CONTROL_PORT GPIOD +#define RESET_PIN 0 // N0 +#define FEED_HOLD_PIN 1 // N1 +#define CYCLE_START_PIN 2 // N2 +#if SAFETY_DOOR_ENABLE +#define SAFETY_DOOR_PIN 1 +#endif +#define CONTROL_INMODE GPIO_SHIFT6 + +// Define probe switch input pin. +#define PROBE_PORT GPIOD +#define PROBE_PIN 12 // N15 + + +/* EOF */ diff --git a/STM32F4xx Debug.launch b/STM32F4xx Debug.launch new file mode 100644 index 00000000..66034b56 --- /dev/null +++ b/STM32F4xx Debug.launch @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Src/f4mpv5_hosoi.c b/Src/f4mpv5_hosoi.c new file mode 100644 index 00000000..8132ec64 --- /dev/null +++ b/Src/f4mpv5_hosoi.c @@ -0,0 +1,376 @@ +/* + * my_plugin.c + * + * Created on: Dec 10, 2021 + * Author: tai + */ + +#include "main.h" +#include "driver.h" +#include "serial.h" + +#include "grbl/limits.h" +#include "grbl/protocol.h" +#include "grbl/motor_pins.h" +#include "grbl/pin_bits_masks.h" +#include "grbl/stepdir_map.h" + +#define PU1_Pin GPIO_PIN_2 +#define PU1_GPIO_Port GPIOA +#define DR1_Pin GPIO_PIN_3 +#define DR1_GPIO_Port GPIOA +#define LedG_Pin GPIO_PIN_6 +#define LedG_GPIO_Port GPIOC +#define LedB_Pin GPIO_PIN_7 +#define LedB_GPIO_Port GPIOC +#define LedR_Pin GPIO_PIN_8 +#define LedR_GPIO_Port GPIOC + +#define led1 GPIOC,GPIO_PIN_6 //GR +#define led2 GPIOC,GPIO_PIN_7 //BL +#define led3 GPIOC,GPIO_PIN_8 //RD +#define led1on HAL_GPIO_WritePin(led1,GPIO_PIN_SET) +#define led2on HAL_GPIO_WritePin(led2,GPIO_PIN_SET) +#define led3on HAL_GPIO_WritePin(led3,GPIO_PIN_SET) +#define led1off HAL_GPIO_WritePin(led1,GPIO_PIN_RESET) +#define led2off HAL_GPIO_WritePin(led2,GPIO_PIN_RESET) +#define led3off HAL_GPIO_WritePin(led3,GPIO_PIN_RESET) +#define led1toggle HAL_GPIO_TogglePin(led1); +#define led2toggle HAL_GPIO_TogglePin(led2); +#define led3toggle HAL_GPIO_TogglePin(led3); + +#define X0 GPIOE,GPIO_PIN_0 +#define X1 GPIOE,GPIO_PIN_1 +#define X2 GPIOE,GPIO_PIN_2 +#define X3 GPIOE,GPIO_PIN_3 +#define X4 GPIOE,GPIO_PIN_4 +#define X5 GPIOE,GPIO_PIN_5 +#define X6 GPIOE,GPIO_PIN_6 +#define X7 GPIOE,GPIO_PIN_7 +#define X8 GPIOE,GPIO_PIN_8 +#define X9 GPIOE,GPIO_PIN_9 +#define X10 GPIOE,GPIO_PIN_10 +#define X11 GPIOE,GPIO_PIN_11 + +#define X0R HAL_GPIO_ReadPin(X0) +#define X1R HAL_GPIO_ReadPin(X1) +#define X2R HAL_GPIO_ReadPin(X2) +#define X3R HAL_GPIO_ReadPin(X3) +#define X4R HAL_GPIO_ReadPin(X4) +#define X5R HAL_GPIO_ReadPin(X5) +#define X6R HAL_GPIO_ReadPin(X6) +#define X7R HAL_GPIO_ReadPin(X7) +#define X8R HAL_GPIO_ReadPin(X8) +#define X9R HAL_GPIO_ReadPin(X9) +#define X10R HAL_GPIO_ReadPin(X10) +#define X11R HAL_GPIO_ReadPin(X11) + +#define X0set HAL_GPIO_WritePin(X0, GPIO_PIN_SET); +#define X1set HAL_GPIO_WritePin(X1, GPIO_PIN_SET); +#define X2set HAL_GPIO_WritePin(X2, GPIO_PIN_SET); +#define X3set HAL_GPIO_WritePin(X3, GPIO_PIN_SET); +#define X4set HAL_GPIO_WritePin(X4, GPIO_PIN_SET); +#define X5set HAL_GPIO_WritePin(X5, GPIO_PIN_SET); +#define X6set HAL_GPIO_WritePin(X6, GPIO_PIN_SET); +#define X7set HAL_GPIO_WritePin(X7, GPIO_PIN_SET); +#define X8set HAL_GPIO_WritePin(X8, GPIO_PIN_SET); +#define X9set HAL_GPIO_WritePin(X9, GPIO_PIN_SET); +#define X10set HAL_GPIO_WritePin(X10, GPIO_PIN_SET); +#define X11set HAL_GPIO_WritePin(X11, GPIO_PIN_SET); + +#define X0clr HAL_GPIO_WritePin(X0, GPIO_PIN_RESET); +#define X1clr HAL_GPIO_WritePin(X1, GPIO_PIN_RESET); +#define X2clr HAL_GPIO_WritePin(X2, GPIO_PIN_RESET); +#define X3clr HAL_GPIO_WritePin(X3, GPIO_PIN_RESET); +#define X4clr HAL_GPIO_WritePin(X4, GPIO_PIN_RESET); +#define X5clr HAL_GPIO_WritePin(X5, GPIO_PIN_RESET); +#define X6clr HAL_GPIO_WritePin(X6, GPIO_PIN_RESET); +#define X7clr HAL_GPIO_WritePin(X7, GPIO_PIN_RESET); +#define X8clr HAL_GPIO_WritePin(X8, GPIO_PIN_RESET); +#define X9clr HAL_GPIO_WritePin(X9, GPIO_PIN_RESET); +#define X10clr HAL_GPIO_WritePin(X10, GPIO_PIN_RESET); +#define X11clr HAL_GPIO_WritePin(X11, GPIO_PIN_RESET); + +#define X0tog HAL_GPIO_TogglePin(X0); +#define X1tog HAL_GPIO_TogglePin(X1); +#define X2tog HAL_GPIO_TogglePin(X2); +#define X3tog HAL_GPIO_TogglePin(X3); +#define X4tog HAL_GPIO_TogglePin(X4); +#define X5tog HAL_GPIO_TogglePin(X5); +#define X6tog HAL_GPIO_TogglePin(X6); +#define X7tog HAL_GPIO_TogglePin(X7); +#define X8tog HAL_GPIO_TogglePin(X8); +#define X9tog HAL_GPIO_TogglePin(X9); +#define X10tog HAL_GPIO_TogglePin(X10); +#define X11tog HAL_GPIO_TogglePin(X11); + +TIM_HandleTypeDef htim2; + +static on_report_options_ptr on_report_options; +static on_execute_realtime_ptr on_execute_realtime; + +stepper_t stepper = +{ 0 }; + +static bool encoder_interrupt_busy = false; + +static void MX_TIM2_Init(void); +static void encoder_handler(sys_state_t state); + +// Add info about our plugin to the $I report. +static void on_report_my_options(bool newopt) +{ + on_report_options(newopt); + + if (!newopt) hal.stream.write("[PLUGIN:Blink LED v1.00]" ASCII_EOL); +} + +static void blink_led(sys_state_t state) +{ +// static bool led_on = false; + static uint32_t ms = 0; + + if (hal.get_elapsed_ticks() >= ms) + { + ms = hal.get_elapsed_ticks() + 1000; //ms + +// HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_6); // Green +// HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_7); // Blue + HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_8); // Red + +// Alternative Method +// static bool led_on = false; +// static uint32_t ms = 0; +// +// if (hal.get_elapsed_ticks() >= ms) +// { +// ms = hal.get_elapsed_ticks() + 500; //ms +// led_on = !led_on; +// if (led_on) +// GPIOC->ODR |= GPIO_PIN_13; +// else +// GPIOC->ODR &= ~GPIO_PIN_13; +// } + + } + + on_execute_realtime(state); +} + +//#define SLOW_JOG +#define FAST_JOG + +volatile int step = 0; +volatile uint32_t counter = 0; +volatile int16_t count = 0; +volatile int16_t prev_count = 0; +volatile int16_t delta_count = 0; + +void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) +{ + /* Prevent unused argument(s) compilation warning */ + UNUSED(htim); + +// HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_6); + counter = __HAL_TIM_GET_COUNTER(htim); + count = (int16_t) (counter / 4); + delta_count = count - prev_count; + + if ((delta_count < 0) || (delta_count > 0)) + { + protocol_enqueue_rt_command(encoder_handler); + + } +} + +static void MX_TIM2_Init(void) +{ + TIM_Encoder_InitTypeDef sConfig = + { 0 }; + TIM_MasterConfigTypeDef sMasterConfig = + { 0 }; + htim2.Instance = TIM2; + htim2.Init.Prescaler = 0; + htim2.Init.CounterMode = TIM_COUNTERMODE_UP; + htim2.Init.Period = 4294967295; + htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + sConfig.EncoderMode = TIM_ENCODERMODE_TI12; + sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; + sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; + sConfig.IC1Prescaler = TIM_ICPSC_DIV1; + sConfig.IC1Filter = 0; + sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; + sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; + sConfig.IC2Prescaler = TIM_ICPSC_DIV1; + sConfig.IC2Filter = 0; + if (HAL_TIM_Encoder_Init(&htim2, &sConfig) != HAL_OK) + { + Error_Handler(); + } + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) + { + Error_Handler(); + } +} + +void led_init(void) +{ + GPIO_InitTypeDef GPIO_InitStruct; + + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + + GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8, GPIO_PIN_RESET); +} + +void output_init(void) +{ + + GPIO_InitTypeDef GPIO_InitStruct; + + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + + //X0-X11 + GPIO_InitStruct.Pin = GPIO_PIN_All; + HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); + +////PU1,DR1-PU8-DR8 +// GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5 | GPIO_PIN_6 +// | GPIO_PIN_7; +// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); +// GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3; +// HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); +// GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3; +// HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); +// +////CS0 +// GPIO_InitStruct.Pin = GPIO_PIN_15; +// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); +////KEY +// GPIO_InitStruct.Pin = GPIO_PIN_12; +// HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); +} + +static void encoder_handler(sys_state_t state) +{ + + if (encoder_interrupt_busy == true) return; + + if ((delta_count < 0) || (delta_count > 0)) + { + encoder_interrupt_busy = true; + } + + if (delta_count > 0) + { + +#ifdef SLOW_JOG + + stepper.step_outbits.x = 1; + stepper.dir_outbits.x = 1; + stepper.dir_change = 1; // set when dir_outbits is changed. + hal.stepper.pulse_start(&stepper); + // hal.stepper. + sys.position[0] = sys.position[0] + (stepper.dir_outbits.x ? -1 : 1); + sync_position(); +#endif +#ifdef FAST_JOG + for (uint16_t step = 0; step < 100; step++) + { + stepper.step_outbits.x = 1; + stepper.dir_outbits.x = 1; + stepper.dir_change = 1; // set when dir_outbits is changed. + hal.stepper.pulse_start(&stepper); + + sys.position[0] = sys.position[0] + (stepper.dir_outbits.x ? -1 : 1); + sync_position(); + + hal.delay_ms(1, NULL); + + } + // stepper.step_outbits.x = 1; + // stepper.dir_outbits.x = 1; + // stepper.dir_change = 1; // set when dir_outbits is changed. + // hal.stepper.pulse_start(&stepper); + // sys.position[0] = sys.position[0] + (stepper.dir_outbits.x ? -1 : 1); + // sync_position(); + +#endif + + } + else if (delta_count < 0) + { + +#ifdef SLOW_JOG + stepper.step_outbits.x = 1; + stepper.dir_outbits.x = 0; + stepper.dir_change = 1; // set when dir_outbits is changed. + hal.stepper.pulse_start(&stepper); + sys.position[0] = sys.position[0] + (stepper.dir_outbits.x ? -1 : 1); + sync_position(); +#endif +#ifdef FAST_JOG + for (uint16_t step = 0; step < 100; step++) + { + stepper.step_outbits.x = 1; + stepper.dir_outbits.x = 0; + stepper.dir_change = 1; // set when dir_outbits is changed. + hal.stepper.pulse_start(&stepper); + + sys.position[0] = sys.position[0] + (stepper.dir_outbits.x ? -1 : 1); + sync_position(); + + hal.delay_ms(1, NULL); + + } + +#endif + + } + prev_count = count; + encoder_interrupt_busy = false; +} + +/************************************ + * + * SERIAL PORT + * + ************************************/ + +/************************************ + * + * MAIN CODE + * + ************************************/ +void board_init(void) +{ + // Add info about our plugin to the $I report. + on_report_options = grbl.on_report_options; + grbl.on_report_options = on_report_my_options; + + // Add blink LED function to grblHAL foreground process + led_init(); + on_execute_realtime = grbl.on_execute_realtime; + grbl.on_execute_realtime = blink_led; + + // Enable F4MPV5 outputs + output_init(); + // Enable Steppers + X0set + ; + + // Enable Encoder Timer +// MX_TIM2_Init(); +// HAL_TIM_Encoder_Start_IT(&htim2, TIM_CHANNEL_ALL); + +} + From 85afbe0aecda1a3153cf1c3d6df46f1483adac43 Mon Sep 17 00:00:00 2001 From: atlesg Date: Wed, 22 Dec 2021 16:58:18 +0700 Subject: [PATCH 02/10] Updated to a working state --- .settings/language.settings.xml | 36 ++-- Inc/driver.h | 6 +- Inc/my_machine.h | 6 +- Inc/my_machine_map.h | 5 + STM32F4xx Debug.launch | 1 + Src/driver.c | 13 +- Src/f4mpv5_hosoi.c | 296 +++++++++++++++++++++++++++++++- Src/serial.c | 24 +-- 8 files changed, 342 insertions(+), 45 deletions(-) diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index 1e4afb2e..eec2ee70 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -6,7 +6,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -30,7 +30,7 @@ - + @@ -42,7 +42,7 @@ - + @@ -54,7 +54,7 @@ - + @@ -66,7 +66,7 @@ - + @@ -78,7 +78,7 @@ - + @@ -90,7 +90,7 @@ - + @@ -102,7 +102,7 @@ - + @@ -114,7 +114,7 @@ - + @@ -126,7 +126,7 @@ - + @@ -138,7 +138,7 @@ - + @@ -150,7 +150,7 @@ - + @@ -162,7 +162,7 @@ - + @@ -174,7 +174,7 @@ - + @@ -186,7 +186,7 @@ - + @@ -198,7 +198,7 @@ - + @@ -210,7 +210,7 @@ - + diff --git a/Inc/driver.h b/Inc/driver.h index 6bda8316..8f6b996d 100644 --- a/Inc/driver.h +++ b/Inc/driver.h @@ -305,9 +305,9 @@ #error SD card plugin not supported! #endif -#ifndef I2C_PORT -#define I2C_PORT 2 // GPIOB, SCL_PIN = 10, SDA_PIN = 11 -#endif +//#ifndef I2C_PORT +//#define I2C_PORT 2 // GPIOB, SCL_PIN = 10, SDA_PIN = 11 +//#endif #ifndef SPI_PORT #define SPI_PORT 1 diff --git a/Inc/my_machine.h b/Inc/my_machine.h index dd5103a6..f8965872 100644 --- a/Inc/my_machine.h +++ b/Inc/my_machine.h @@ -49,12 +49,12 @@ // Uncomment to enable. #if !IS_NUCLEO_DEVKIT // The Nucleo boards has an off-chip UART to USB interface. -#define USB_SERIAL_CDC 1 // Serial communication via native USB. +//#define USB_SERIAL_CDC 1 // Serial communication via native USB. #endif //#define BLUETOOTH_ENABLE 1 // Set to 1 for HC-05 module. Requires and claims one auxillary input pin. -#define VFD_ENABLE 99 // Set to 1 or 2 for Huanyang VFD spindle. More here https://github.com/grblHAL/Plugins_spindle +//#define VFD_ENABLE 1 // Set to 1 or 2 for Huanyang VFD spindle. More here https://github.com/grblHAL/Plugins_spindle //#define DUAL_SPINDLE 1 // Uncomment for switching between VFD spindle and PWM output with $32 -#define MODBUS_ENABLE 1 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin. +//#define MODBUS_ENABLE 1 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin. //#define SDCARD_ENABLE 2 // Run gcode programs from SD card. //#define KEYPAD_ENABLE 1 // Set to 1 for I2C keypad, 2 for other input such as serial data //#define ODOMETER_ENABLE 1 // Odometer plugin. diff --git a/Inc/my_machine_map.h b/Inc/my_machine_map.h index d3605c7a..3a08ca86 100644 --- a/Inc/my_machine_map.h +++ b/Inc/my_machine_map.h @@ -127,5 +127,10 @@ #define PROBE_PORT GPIOD #define PROBE_PIN 12 // N15 +// Define main grbl uart port +#define UART_INSTANCE 2 + +// Define I2C Port - avoid confliction with USART3 port +#undef I2C_PORT /* EOF */ diff --git a/STM32F4xx Debug.launch b/STM32F4xx Debug.launch index 66034b56..10671701 100644 --- a/STM32F4xx Debug.launch +++ b/STM32F4xx Debug.launch @@ -75,5 +75,6 @@ + diff --git a/Src/driver.c b/Src/driver.c index 4a3d2dbe..dcecdcdf 100644 --- a/Src/driver.c +++ b/Src/driver.c @@ -1887,7 +1887,7 @@ bool driver_init (void) #else hal.info = "STM32F401CC"; #endif - hal.driver_version = "211209"; + hal.driver_version = "211221"; #ifdef BOARD_NAME hal.board = BOARD_NAME; #endif @@ -1954,8 +1954,8 @@ bool driver_init (void) #if USB_SERIAL_CDC stream_connect(usbInit()); -#else - stream_connect(serialInit(115200)); +#elif !defined(UART_INSTANCE) + stream_connect(serialInit(BAUD_RATE)); #endif #ifdef I2C_PORT @@ -2008,6 +2008,8 @@ bool driver_init (void) hal.driver_cap.probe_pull_up = On; #endif + serialRegisterStreams(); + #ifdef HAS_IOPORTS uint32_t i; @@ -2044,7 +2046,10 @@ bool driver_init (void) board_init(); #endif - serialRegisterStreams(); +#if defined(UART_INSTANCE) && USB_SERIAL_CDC == 0 + if(!stream_connect_instance(0, BAUD_RATE)) + while(true); // Cannot boot if no communication channel is available! +#endif #if ETHERNET_ENABLE enet_init(); diff --git a/Src/f4mpv5_hosoi.c b/Src/f4mpv5_hosoi.c index 8132ec64..e1a0fad3 100644 --- a/Src/f4mpv5_hosoi.c +++ b/Src/f4mpv5_hosoi.c @@ -5,10 +5,13 @@ * Author: tai */ +#include + #include "main.h" #include "driver.h" #include "serial.h" +#include "grbl/hal.h" #include "grbl/limits.h" #include "grbl/protocol.h" #include "grbl/motor_pins.h" @@ -345,6 +348,283 @@ static void encoder_handler(sys_state_t state) * SERIAL PORT * ************************************/ +//#define UART_PORT GPIOD +//#define UART_TX_PIN 8 +//#define UART_RX_PIN 9 +const io_stream_t* serial3Init(uint32_t baud_rate); +const io_stream_t* serial4Init(uint32_t baud_rate); + +static stream_rx_buffer_t rxbuf3 = +{ 0 }; +static stream_tx_buffer_t txbuf3 = +{ 0 }; +static enqueue_realtime_command_ptr enqueue_realtime_command3 = protocol_enqueue_realtime_command; + +#define USART_GRBL USART3 +#define USART_GRBL_IRQHandler USART3_IRQHandler + +static io_stream_properties_t serial[] = +{ +{ + .type = StreamType_Serial, + .instance = 0, + .flags.claimable = On, + .flags.claimed = Off, + .flags.connected = On, + .flags.can_set_baud = + On, + .claim = serialInit }, +#ifdef SERIAL2_MOD +{ + .type = StreamType_Serial, + .instance = 1, + .flags.claimable = On, + .flags.claimed = Off, + .flags.connected = On, + .flags.can_set_baud = On, + .flags.modbus_ready = On, + .claim = serial2Init +} +#endif + + { + .type = StreamType_Serial, + .instance = 2, + .flags.claimable = On, + .flags.claimed = Off, + .flags.connected = On, + .flags.can_set_baud = + On, + .claim = serial3Init }, + + { + .type = StreamType_Serial, + .instance = 3, + .flags.claimable = On, + .flags.claimed = Off, + .flags.connected = On, + .flags.can_set_baud = + On, + .flags.modbus_ready = On, + .claim = serial4Init } + +}; + +// +// Returns number of free characters in serial input buffer +// +static uint16_t serial3RxFree(void) +{ + uint16_t tail = rxbuf3.tail, head = rxbuf3.head; + + return RX_BUFFER_SIZE - BUFCOUNT(head, tail, RX_BUFFER_SIZE); +} + +// +// Flushes the serial input buffer +// +static void serial3RxFlush(void) +{ + rxbuf3.tail = rxbuf3.head; +} + +// +// Flushes and adds a CAN character to the serial input buffer +// +static void serial3RxCancel(void) +{ + rxbuf3.data[rxbuf3.head] = ASCII_CAN; + rxbuf3.tail = rxbuf3.head; + rxbuf3.head = BUFNEXT(rxbuf3.head, rxbuf3); +} + +// +// Writes a character to the serial output stream +// +static bool serial3PutC(const char c) +{ + uint16_t next_head = BUFNEXT(txbuf3.head, txbuf3); // Get pointer to next free slot in buffer + + while (txbuf3.tail == next_head) + { // While TX buffer full + if (!hal.stream_blocking_callback()) // check if blocking for space, + return false; // exit if not (leaves TX buffer in an inconsistent state) + } + txbuf3.data[txbuf3.head] = c; // Add data to buffer, + txbuf3.head = next_head; // update head pointer and + USART_GRBL->CR1 |= USART_CR1_TXEIE; // enable TX interrupts + + return true; +} + +// +// Writes a null terminated string to the serial output stream, blocks if buffer full +// +static void serial3WriteS(const char *s) +{ + char c, *ptr = (char*) s; + + while ((c = *ptr++) != '\0') + serial3PutC(c); +} + +// +// Writes a number of characters from string to the serial output stream followed by EOL, blocks if buffer full +// +static void serial3Write(const char *s, uint16_t length) +{ + char *ptr = (char*) s; + + while (length--) + serial3PutC(*ptr++); +} + +// +// serialGetC - returns -1 if no data available +// +static int16_t serial3GetC(void) +{ + uint_fast16_t tail = rxbuf3.tail; // Get buffer pointer + + if (tail == rxbuf3.head) return -1; // no data available + + char data = rxbuf3.data[tail]; // Get next character + rxbuf3.tail = BUFNEXT(tail, rxbuf3); // and update pointer + + return (int16_t) data; +} + +static bool serial3SuspendInput(bool suspend) +{ + return stream_rx_suspend(&rxbuf3, suspend); +} + +static bool serial3SetBaudRate(uint32_t baud_rate) +{ +// if USART 1/6/9/10 → HAL_RCC_GetPCLK2Freq() - else: HAL_RCC_GetPCLK1Freq() + + USART_GRBL->CR1 = USART_CR1_RE | USART_CR1_TE; + USART_GRBL->BRR = UART_BRR_SAMPLING16(HAL_RCC_GetPCLK1Freq(), baud_rate); + USART_GRBL->CR1 |= (USART_CR1_UE | USART_CR1_RXNEIE); + + return true; +} + +static bool serial3Disable(bool disable) +{ + if (disable) USART_GRBL->CR1 &= ~USART_CR1_RXNEIE; + else USART_GRBL->CR1 |= USART_CR1_RXNEIE; + + return true; +} + +static bool serial3EnqueueRtCommand(char c) +{ + return enqueue_realtime_command3(c); +} + +static enqueue_realtime_command_ptr serial3SetRtHandler(enqueue_realtime_command_ptr handler) +{ + enqueue_realtime_command_ptr prev = enqueue_realtime_command3; + + if (handler) enqueue_realtime_command3 = handler; + + return prev; +} + +const io_stream_t* serial3Init(uint32_t baud_rate) +{ + + static const io_stream_t stream = + { + .type = StreamType_Serial, + .state.connected = On, + .read = serial3GetC, + .write = serial3WriteS, + .write_n = serial3Write, + .write_char = serial3PutC, + .enqueue_rt_command = serial3EnqueueRtCommand, + .get_rx_buffer_free = serial3RxFree, +// .get_rx_buffer_count = serial3RxCount, +// .get_tx_buffer_count = serial3TxCount, +// .reset_write_buffer = serial3TxFlush, + .reset_read_buffer = serial3RxFlush, + .cancel_read_buffer = serial3RxCancel, + .suspend_read = serial3SuspendInput, + .disable_rx = serial3Disable, + .set_baud_rate = serial3SetBaudRate, + .set_enqueue_rt_handler = serial3SetRtHandler }; + + if (serial[0].flags.claimed) return NULL; + + serial[0].flags.claimed = On; + + GPIO_InitTypeDef GPIO_InitStructure = + { 0 }; + + GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; + GPIO_InitStructure.Pull = GPIO_NOPULL; + GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + + __HAL_RCC_GPIOB_CLK_ENABLE(); // USART3 Port + __HAL_RCC_USART3_CLK_ENABLE(); + + GPIO_InitStructure.Pin = GPIO_PIN_10 | GPIO_PIN_11; + GPIO_InitStructure.Alternate = GPIO_AF7_USART1; + HAL_GPIO_Init(GPIOB, &GPIO_InitStructure); + + serial3SetBaudRate(baud_rate); + + HAL_NVIC_SetPriority(USART3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(USART3_IRQn); + + static const periph_pin_t tx = + { .function = Output_TX, .group = PinGroup_UART3, .port = GPIOB, .pin = 10, .mode = + { .mask = PINMODE_OUTPUT }, .description = "UART3" }; + + static const periph_pin_t rx = + { .function = Input_RX, .group = PinGroup_UART3, .port = GPIOB, .pin = 11, .mode = + { .mask = PINMODE_NONE }, .description = "UART3" }; + + hal.periph_port.register_pin(&rx); + hal.periph_port.register_pin(&tx); + + return &stream; +} + +// SERIAL 4 +const io_stream_t* serial4Init(uint32_t baud_rate) +{ + +} + +void USART_GRBL_IRQHandler(void) +{ + if (USART_GRBL->SR & USART_SR_RXNE) + { + uint32_t data = USART_GRBL->DR; + if (!enqueue_realtime_command3((char) data)) + { // Check and strip realtime commands... + uint16_t next_head = BUFNEXT(rxbuf3.head, rxbuf3); // Get and increment buffer pointer + if (next_head == rxbuf3.tail) // If buffer full + rxbuf3.overflow = 1; // flag overflow + else + { + rxbuf3.data[rxbuf3.head] = (char) data; // if not add data to buffer + rxbuf3.head = next_head; // and update pointer + } + } + } + + if ((USART_GRBL->SR & USART_SR_TXE) && (USART_GRBL->CR1 & USART_CR1_TXEIE)) + { + uint_fast16_t tail = txbuf3.tail; // Get buffer pointer + USART_GRBL->DR = txbuf3.data[tail]; // Send next character + txbuf3.tail = tail = BUFNEXT(tail, txbuf3); // and increment pointer + if (tail == txbuf3.head) // If buffer empty then + USART_GRBL->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt + } +} /************************************ * @@ -353,22 +633,28 @@ static void encoder_handler(sys_state_t state) ************************************/ void board_init(void) { - // Add info about our plugin to the $I report. +// Add info about our plugin to the $I report. on_report_options = grbl.on_report_options; grbl.on_report_options = on_report_my_options; - // Add blink LED function to grblHAL foreground process +// Add blink LED function to grblHAL foreground process led_init(); on_execute_realtime = grbl.on_execute_realtime; grbl.on_execute_realtime = blink_led; - // Enable F4MPV5 outputs +// Enable F4MPV5 outputs output_init(); - // Enable Steppers +// Enable Steppers X0set ; - // Enable Encoder Timer +// Enable UART + static io_stream_details_t streams = + { .n_streams = sizeof(serial) / sizeof(io_stream_properties_t), .streams = serial, }; + + stream_register_streams(&streams); + +// Enable Encoder Timer // MX_TIM2_Init(); // HAL_TIM_Encoder_Start_IT(&htim2, TIM_CHANNEL_ALL); diff --git a/Src/serial.c b/Src/serial.c index 6cd5f701..33656004 100644 --- a/Src/serial.c +++ b/Src/serial.c @@ -268,7 +268,7 @@ const io_stream_t *serialInit (uint32_t baud_rate) .port = GPIOA, .pin = 2, .mode = { .mask = PINMODE_OUTPUT }, - .description = "Primary UART" + .description = "UART1" }; static const periph_pin_t rx = { .function = Input_RX, @@ -276,7 +276,7 @@ const io_stream_t *serialInit (uint32_t baud_rate) .port = GPIOA, .pin = 3, .mode = { .mask = PINMODE_NONE }, - .description = "Primary UART" + .description = "UART1" }; @@ -299,7 +299,7 @@ const io_stream_t *serialInit (uint32_t baud_rate) .port = GPIOA, .pin = 9, .mode = { .mask = PINMODE_OUTPUT }, - .description = "Primary UART" + .description = "UART1" }; static const periph_pin_t rx = { @@ -308,7 +308,7 @@ const io_stream_t *serialInit (uint32_t baud_rate) .port = GPIOA, .pin = 10, .mode = { .mask = PINMODE_NONE }, - .description = "Primary UART" + .description = "UART1" }; #endif @@ -566,20 +566,20 @@ const io_stream_t *serial2Init (uint32_t baud_rate) static const periph_pin_t tx = { .function = Output_TX, - .group = PinGroup_UART2, + .group = PinGroup_UART + stream.instance, .port = GPIOA, .pin = 9, .mode = { .mask = PINMODE_OUTPUT }, - .description = "Secondary UART" + .description = "UART2" }; static const periph_pin_t rx = { .function = Input_RX, - .group = PinGroup_UART2, + .group = PinGroup_UART + stream.instance, .port = GPIOA, .pin = 10, .mode = { .mask = PINMODE_NONE }, - .description = "Secondary UART" + .description = "UART2" }; #else @@ -603,20 +603,20 @@ const io_stream_t *serial2Init (uint32_t baud_rate) static const periph_pin_t tx = { .function = Output_TX, - .group = PinGroup_UART2, + .group = PinGroup_UART + stream.instance, .port = GPIOA, .pin = 2, .mode = { .mask = PINMODE_OUTPUT }, - .description = "Secondary UART" + .description = "UART2" }; static const periph_pin_t rx = { .function = Input_RX, - .group = PinGroup_UART2, + .group = PinGroup_UART + stream.instance, .port = GPIOA, .pin = 3, .mode = { .mask = PINMODE_NONE }, - .description = "Secondary UART" + .description = "UART2" }; #endif From 9bd5560a503a45d5223599e88a967f7a6762d27e Mon Sep 17 00:00:00 2001 From: atlesg Date: Thu, 23 Dec 2021 09:34:39 +0700 Subject: [PATCH 03/10] Update --- Inc/my_machine_map.h | 2 +- Src/driver.c | 2 +- Src/f4mpv5_hosoi.c | 49 ++++++++++++++++++++++---------------------- 3 files changed, 26 insertions(+), 27 deletions(-) diff --git a/Inc/my_machine_map.h b/Inc/my_machine_map.h index 3a08ca86..857b9302 100644 --- a/Inc/my_machine_map.h +++ b/Inc/my_machine_map.h @@ -131,6 +131,6 @@ #define UART_INSTANCE 2 // Define I2C Port - avoid confliction with USART3 port -#undef I2C_PORT +//#undef I2C_PORT /* EOF */ diff --git a/Src/driver.c b/Src/driver.c index dcecdcdf..6d066621 100644 --- a/Src/driver.c +++ b/Src/driver.c @@ -2047,7 +2047,7 @@ bool driver_init (void) #endif #if defined(UART_INSTANCE) && USB_SERIAL_CDC == 0 - if(!stream_connect_instance(0, BAUD_RATE)) + if(!stream_connect_instance(UART_INSTANCE, BAUD_RATE)) while(true); // Cannot boot if no communication channel is available! #endif diff --git a/Src/f4mpv5_hosoi.c b/Src/f4mpv5_hosoi.c index e1a0fad3..726a25c7 100644 --- a/Src/f4mpv5_hosoi.c +++ b/Src/f4mpv5_hosoi.c @@ -600,30 +600,30 @@ const io_stream_t* serial4Init(uint32_t baud_rate) void USART_GRBL_IRQHandler(void) { - if (USART_GRBL->SR & USART_SR_RXNE) - { - uint32_t data = USART_GRBL->DR; - if (!enqueue_realtime_command3((char) data)) - { // Check and strip realtime commands... - uint16_t next_head = BUFNEXT(rxbuf3.head, rxbuf3); // Get and increment buffer pointer - if (next_head == rxbuf3.tail) // If buffer full - rxbuf3.overflow = 1; // flag overflow - else - { - rxbuf3.data[rxbuf3.head] = (char) data; // if not add data to buffer - rxbuf3.head = next_head; // and update pointer - } - } - } - - if ((USART_GRBL->SR & USART_SR_TXE) && (USART_GRBL->CR1 & USART_CR1_TXEIE)) - { - uint_fast16_t tail = txbuf3.tail; // Get buffer pointer - USART_GRBL->DR = txbuf3.data[tail]; // Send next character - txbuf3.tail = tail = BUFNEXT(tail, txbuf3); // and increment pointer - if (tail == txbuf3.head) // If buffer empty then - USART_GRBL->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt - } +// if (USART_GRBL->SR & USART_SR_RXNE) +// { +// uint32_t data = USART_GRBL->DR; +// if (!enqueue_realtime_command3((char) data)) +// { // Check and strip realtime commands... +// uint16_t next_head = BUFNEXT(rxbuf3.head, rxbuf3); // Get and increment buffer pointer +// if (next_head == rxbuf3.tail) // If buffer full +// rxbuf3.overflow = 1; // flag overflow +// else +// { +// rxbuf3.data[rxbuf3.head] = (char) data; // if not add data to buffer +// rxbuf3.head = next_head; // and update pointer +// } +// } +// } +// +// if ((USART_GRBL->SR & USART_SR_TXE) && (USART_GRBL->CR1 & USART_CR1_TXEIE)) +// { +// uint_fast16_t tail = txbuf3.tail; // Get buffer pointer +// USART_GRBL->DR = txbuf3.data[tail]; // Send next character +// txbuf3.tail = tail = BUFNEXT(tail, txbuf3); // and increment pointer +// if (tail == txbuf3.head) // If buffer empty then +// USART_GRBL->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt +// } } /************************************ @@ -651,7 +651,6 @@ void board_init(void) // Enable UART static io_stream_details_t streams = { .n_streams = sizeof(serial) / sizeof(io_stream_properties_t), .streams = serial, }; - stream_register_streams(&streams); // Enable Encoder Timer From 024c59ba9217df0050b732fd683111db92e93c2d Mon Sep 17 00:00:00 2001 From: atlesg Date: Thu, 23 Dec 2021 14:15:22 +0700 Subject: [PATCH 04/10] Updated to a working state - Stream registering okay - TODO: spindle control using modbus + 2 wires - TODO: jogging using rotary encoder --- .cproject | 2 +- Inc/my_machine_map.h | 2 +- Src/f4mpv4_hosoi.h | 100 ++++++++++ Src/f4mpv5_hosoi.c | 450 +++++++++++++++++++++++++++---------------- 4 files changed, 389 insertions(+), 165 deletions(-) create mode 100644 Src/f4mpv4_hosoi.h diff --git a/.cproject b/.cproject index b7d7af32..4fe96548 100644 --- a/.cproject +++ b/.cproject @@ -120,7 +120,7 @@ - + diff --git a/Inc/my_machine_map.h b/Inc/my_machine_map.h index 857b9302..6bc758b7 100644 --- a/Inc/my_machine_map.h +++ b/Inc/my_machine_map.h @@ -128,7 +128,7 @@ #define PROBE_PIN 12 // N15 // Define main grbl uart port -#define UART_INSTANCE 2 +#define UART_INSTANCE 0 // Define I2C Port - avoid confliction with USART3 port //#undef I2C_PORT diff --git a/Src/f4mpv4_hosoi.h b/Src/f4mpv4_hosoi.h new file mode 100644 index 00000000..77d0bc48 --- /dev/null +++ b/Src/f4mpv4_hosoi.h @@ -0,0 +1,100 @@ +/* + * f4mpv4_hosoi.h + * + * Created on: Dec 23, 2021 + * Author: tai + */ + +#ifndef F4MPV4_HOSOI_H_ +#define F4MPV4_HOSOI_H_ + +#define PU1_Pin GPIO_PIN_2 +#define PU1_GPIO_Port GPIOA +#define DR1_Pin GPIO_PIN_3 +#define DR1_GPIO_Port GPIOA +#define LedG_Pin GPIO_PIN_6 +#define LedG_GPIO_Port GPIOC +#define LedB_Pin GPIO_PIN_7 +#define LedB_GPIO_Port GPIOC +#define LedR_Pin GPIO_PIN_8 +#define LedR_GPIO_Port GPIOC + +#define led1 GPIOC,GPIO_PIN_6 //GR +#define led2 GPIOC,GPIO_PIN_7 //BL +#define led3 GPIOC,GPIO_PIN_8 //RD +#define led1on HAL_GPIO_WritePin(led1,GPIO_PIN_SET) +#define led2on HAL_GPIO_WritePin(led2,GPIO_PIN_SET) +#define led3on HAL_GPIO_WritePin(led3,GPIO_PIN_SET) +#define led1off HAL_GPIO_WritePin(led1,GPIO_PIN_RESET) +#define led2off HAL_GPIO_WritePin(led2,GPIO_PIN_RESET) +#define led3off HAL_GPIO_WritePin(led3,GPIO_PIN_RESET) +#define led1toggle HAL_GPIO_TogglePin(led1); +#define led2toggle HAL_GPIO_TogglePin(led2); +#define led3toggle HAL_GPIO_TogglePin(led3); + +#define X0 GPIOE,GPIO_PIN_0 +#define X1 GPIOE,GPIO_PIN_1 +#define X2 GPIOE,GPIO_PIN_2 +#define X3 GPIOE,GPIO_PIN_3 +#define X4 GPIOE,GPIO_PIN_4 +#define X5 GPIOE,GPIO_PIN_5 +#define X6 GPIOE,GPIO_PIN_6 +#define X7 GPIOE,GPIO_PIN_7 +#define X8 GPIOE,GPIO_PIN_8 +#define X9 GPIOE,GPIO_PIN_9 +#define X10 GPIOE,GPIO_PIN_10 +#define X11 GPIOE,GPIO_PIN_11 + +#define X0R HAL_GPIO_ReadPin(X0) +#define X1R HAL_GPIO_ReadPin(X1) +#define X2R HAL_GPIO_ReadPin(X2) +#define X3R HAL_GPIO_ReadPin(X3) +#define X4R HAL_GPIO_ReadPin(X4) +#define X5R HAL_GPIO_ReadPin(X5) +#define X6R HAL_GPIO_ReadPin(X6) +#define X7R HAL_GPIO_ReadPin(X7) +#define X8R HAL_GPIO_ReadPin(X8) +#define X9R HAL_GPIO_ReadPin(X9) +#define X10R HAL_GPIO_ReadPin(X10) +#define X11R HAL_GPIO_ReadPin(X11) + +#define X0set HAL_GPIO_WritePin(X0, GPIO_PIN_SET); +#define X1set HAL_GPIO_WritePin(X1, GPIO_PIN_SET); +#define X2set HAL_GPIO_WritePin(X2, GPIO_PIN_SET); +#define X3set HAL_GPIO_WritePin(X3, GPIO_PIN_SET); +#define X4set HAL_GPIO_WritePin(X4, GPIO_PIN_SET); +#define X5set HAL_GPIO_WritePin(X5, GPIO_PIN_SET); +#define X6set HAL_GPIO_WritePin(X6, GPIO_PIN_SET); +#define X7set HAL_GPIO_WritePin(X7, GPIO_PIN_SET); +#define X8set HAL_GPIO_WritePin(X8, GPIO_PIN_SET); +#define X9set HAL_GPIO_WritePin(X9, GPIO_PIN_SET); +#define X10set HAL_GPIO_WritePin(X10, GPIO_PIN_SET); +#define X11set HAL_GPIO_WritePin(X11, GPIO_PIN_SET); + +#define X0clr HAL_GPIO_WritePin(X0, GPIO_PIN_RESET); +#define X1clr HAL_GPIO_WritePin(X1, GPIO_PIN_RESET); +#define X2clr HAL_GPIO_WritePin(X2, GPIO_PIN_RESET); +#define X3clr HAL_GPIO_WritePin(X3, GPIO_PIN_RESET); +#define X4clr HAL_GPIO_WritePin(X4, GPIO_PIN_RESET); +#define X5clr HAL_GPIO_WritePin(X5, GPIO_PIN_RESET); +#define X6clr HAL_GPIO_WritePin(X6, GPIO_PIN_RESET); +#define X7clr HAL_GPIO_WritePin(X7, GPIO_PIN_RESET); +#define X8clr HAL_GPIO_WritePin(X8, GPIO_PIN_RESET); +#define X9clr HAL_GPIO_WritePin(X9, GPIO_PIN_RESET); +#define X10clr HAL_GPIO_WritePin(X10, GPIO_PIN_RESET); +#define X11clr HAL_GPIO_WritePin(X11, GPIO_PIN_RESET); + +#define X0tog HAL_GPIO_TogglePin(X0); +#define X1tog HAL_GPIO_TogglePin(X1); +#define X2tog HAL_GPIO_TogglePin(X2); +#define X3tog HAL_GPIO_TogglePin(X3); +#define X4tog HAL_GPIO_TogglePin(X4); +#define X5tog HAL_GPIO_TogglePin(X5); +#define X6tog HAL_GPIO_TogglePin(X6); +#define X7tog HAL_GPIO_TogglePin(X7); +#define X8tog HAL_GPIO_TogglePin(X8); +#define X9tog HAL_GPIO_TogglePin(X9); +#define X10tog HAL_GPIO_TogglePin(X10); +#define X11tog HAL_GPIO_TogglePin(X11); + +#endif /* F4MPV4_HOSOI_H_ */ diff --git a/Src/f4mpv5_hosoi.c b/Src/f4mpv5_hosoi.c index 726a25c7..1e8ad137 100644 --- a/Src/f4mpv5_hosoi.c +++ b/Src/f4mpv5_hosoi.c @@ -7,6 +7,8 @@ #include +#include "f4mpv4_hosoi.h" + #include "main.h" #include "driver.h" #include "serial.h" @@ -18,95 +20,6 @@ #include "grbl/pin_bits_masks.h" #include "grbl/stepdir_map.h" -#define PU1_Pin GPIO_PIN_2 -#define PU1_GPIO_Port GPIOA -#define DR1_Pin GPIO_PIN_3 -#define DR1_GPIO_Port GPIOA -#define LedG_Pin GPIO_PIN_6 -#define LedG_GPIO_Port GPIOC -#define LedB_Pin GPIO_PIN_7 -#define LedB_GPIO_Port GPIOC -#define LedR_Pin GPIO_PIN_8 -#define LedR_GPIO_Port GPIOC - -#define led1 GPIOC,GPIO_PIN_6 //GR -#define led2 GPIOC,GPIO_PIN_7 //BL -#define led3 GPIOC,GPIO_PIN_8 //RD -#define led1on HAL_GPIO_WritePin(led1,GPIO_PIN_SET) -#define led2on HAL_GPIO_WritePin(led2,GPIO_PIN_SET) -#define led3on HAL_GPIO_WritePin(led3,GPIO_PIN_SET) -#define led1off HAL_GPIO_WritePin(led1,GPIO_PIN_RESET) -#define led2off HAL_GPIO_WritePin(led2,GPIO_PIN_RESET) -#define led3off HAL_GPIO_WritePin(led3,GPIO_PIN_RESET) -#define led1toggle HAL_GPIO_TogglePin(led1); -#define led2toggle HAL_GPIO_TogglePin(led2); -#define led3toggle HAL_GPIO_TogglePin(led3); - -#define X0 GPIOE,GPIO_PIN_0 -#define X1 GPIOE,GPIO_PIN_1 -#define X2 GPIOE,GPIO_PIN_2 -#define X3 GPIOE,GPIO_PIN_3 -#define X4 GPIOE,GPIO_PIN_4 -#define X5 GPIOE,GPIO_PIN_5 -#define X6 GPIOE,GPIO_PIN_6 -#define X7 GPIOE,GPIO_PIN_7 -#define X8 GPIOE,GPIO_PIN_8 -#define X9 GPIOE,GPIO_PIN_9 -#define X10 GPIOE,GPIO_PIN_10 -#define X11 GPIOE,GPIO_PIN_11 - -#define X0R HAL_GPIO_ReadPin(X0) -#define X1R HAL_GPIO_ReadPin(X1) -#define X2R HAL_GPIO_ReadPin(X2) -#define X3R HAL_GPIO_ReadPin(X3) -#define X4R HAL_GPIO_ReadPin(X4) -#define X5R HAL_GPIO_ReadPin(X5) -#define X6R HAL_GPIO_ReadPin(X6) -#define X7R HAL_GPIO_ReadPin(X7) -#define X8R HAL_GPIO_ReadPin(X8) -#define X9R HAL_GPIO_ReadPin(X9) -#define X10R HAL_GPIO_ReadPin(X10) -#define X11R HAL_GPIO_ReadPin(X11) - -#define X0set HAL_GPIO_WritePin(X0, GPIO_PIN_SET); -#define X1set HAL_GPIO_WritePin(X1, GPIO_PIN_SET); -#define X2set HAL_GPIO_WritePin(X2, GPIO_PIN_SET); -#define X3set HAL_GPIO_WritePin(X3, GPIO_PIN_SET); -#define X4set HAL_GPIO_WritePin(X4, GPIO_PIN_SET); -#define X5set HAL_GPIO_WritePin(X5, GPIO_PIN_SET); -#define X6set HAL_GPIO_WritePin(X6, GPIO_PIN_SET); -#define X7set HAL_GPIO_WritePin(X7, GPIO_PIN_SET); -#define X8set HAL_GPIO_WritePin(X8, GPIO_PIN_SET); -#define X9set HAL_GPIO_WritePin(X9, GPIO_PIN_SET); -#define X10set HAL_GPIO_WritePin(X10, GPIO_PIN_SET); -#define X11set HAL_GPIO_WritePin(X11, GPIO_PIN_SET); - -#define X0clr HAL_GPIO_WritePin(X0, GPIO_PIN_RESET); -#define X1clr HAL_GPIO_WritePin(X1, GPIO_PIN_RESET); -#define X2clr HAL_GPIO_WritePin(X2, GPIO_PIN_RESET); -#define X3clr HAL_GPIO_WritePin(X3, GPIO_PIN_RESET); -#define X4clr HAL_GPIO_WritePin(X4, GPIO_PIN_RESET); -#define X5clr HAL_GPIO_WritePin(X5, GPIO_PIN_RESET); -#define X6clr HAL_GPIO_WritePin(X6, GPIO_PIN_RESET); -#define X7clr HAL_GPIO_WritePin(X7, GPIO_PIN_RESET); -#define X8clr HAL_GPIO_WritePin(X8, GPIO_PIN_RESET); -#define X9clr HAL_GPIO_WritePin(X9, GPIO_PIN_RESET); -#define X10clr HAL_GPIO_WritePin(X10, GPIO_PIN_RESET); -#define X11clr HAL_GPIO_WritePin(X11, GPIO_PIN_RESET); - -#define X0tog HAL_GPIO_TogglePin(X0); -#define X1tog HAL_GPIO_TogglePin(X1); -#define X2tog HAL_GPIO_TogglePin(X2); -#define X3tog HAL_GPIO_TogglePin(X3); -#define X4tog HAL_GPIO_TogglePin(X4); -#define X5tog HAL_GPIO_TogglePin(X5); -#define X6tog HAL_GPIO_TogglePin(X6); -#define X7tog HAL_GPIO_TogglePin(X7); -#define X8tog HAL_GPIO_TogglePin(X8); -#define X9tog HAL_GPIO_TogglePin(X9); -#define X10tog HAL_GPIO_TogglePin(X10); -#define X11tog HAL_GPIO_TogglePin(X11); - TIM_HandleTypeDef htim2; static on_report_options_ptr on_report_options; @@ -178,12 +91,6 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) counter = __HAL_TIM_GET_COUNTER(htim); count = (int16_t) (counter / 4); delta_count = count - prev_count; - - if ((delta_count < 0) || (delta_count > 0)) - { - protocol_enqueue_rt_command(encoder_handler); - - } } static void MX_TIM2_Init(void) @@ -348,9 +255,6 @@ static void encoder_handler(sys_state_t state) * SERIAL PORT * ************************************/ -//#define UART_PORT GPIOD -//#define UART_TX_PIN 8 -//#define UART_RX_PIN 9 const io_stream_t* serial3Init(uint32_t baud_rate); const io_stream_t* serial4Init(uint32_t baud_rate); @@ -367,46 +271,24 @@ static io_stream_properties_t serial[] = { { .type = StreamType_Serial, - .instance = 0, + .instance = 2, .flags.claimable = On, .flags.claimed = Off, .flags.connected = On, .flags.can_set_baud = On, - .claim = serialInit }, -#ifdef SERIAL2_MOD -{ - .type = StreamType_Serial, - .instance = 1, - .flags.claimable = On, - .flags.claimed = Off, - .flags.connected = On, - .flags.can_set_baud = On, - .flags.modbus_ready = On, - .claim = serial2Init -} -#endif - - { - .type = StreamType_Serial, - .instance = 2, - .flags.claimable = On, - .flags.claimed = Off, - .flags.connected = On, - .flags.can_set_baud = - On, - .claim = serial3Init }, + .claim = serial3Init }, - { - .type = StreamType_Serial, - .instance = 3, - .flags.claimable = On, - .flags.claimed = Off, - .flags.connected = On, - .flags.can_set_baud = - On, - .flags.modbus_ready = On, - .claim = serial4Init } +{ + .type = StreamType_Serial, + .instance = 3, + .flags.claimable = On, + .flags.claimed = Off, + .flags.connected = On, + .flags.can_set_baud = + On, + .flags.modbus_ready = On, + .claim = serial4Init } }; @@ -443,15 +325,15 @@ static void serial3RxCancel(void) // static bool serial3PutC(const char c) { - uint16_t next_head = BUFNEXT(txbuf3.head, txbuf3); // Get pointer to next free slot in buffer + uint16_t next_head = BUFNEXT(txbuf3.head, txbuf3); // Get pointer to next free slot in buffer while (txbuf3.tail == next_head) { // While TX buffer full - if (!hal.stream_blocking_callback()) // check if blocking for space, - return false; // exit if not (leaves TX buffer in an inconsistent state) + if (!hal.stream_blocking_callback()) // check if blocking for space, + return false; // exit if not (leaves TX buffer in an inconsistent state) } - txbuf3.data[txbuf3.head] = c; // Add data to buffer, - txbuf3.head = next_head; // update head pointer and + txbuf3.data[txbuf3.head] = c; // Add data to buffer, + txbuf3.head = next_head; // update head pointer and USART_GRBL->CR1 |= USART_CR1_TXEIE; // enable TX interrupts return true; @@ -536,15 +418,9 @@ const io_stream_t* serial3Init(uint32_t baud_rate) { static const io_stream_t stream = - { - .type = StreamType_Serial, - .state.connected = On, - .read = serial3GetC, - .write = serial3WriteS, - .write_n = serial3Write, - .write_char = serial3PutC, - .enqueue_rt_command = serial3EnqueueRtCommand, - .get_rx_buffer_free = serial3RxFree, + { .type = StreamType_Serial, .state.connected = On, .read = serial3GetC, .write = serial3WriteS, .write_n = + serial3Write, .write_char = serial3PutC, .enqueue_rt_command = serial3EnqueueRtCommand, .get_rx_buffer_free = + serial3RxFree, // .get_rx_buffer_count = serial3RxCount, // .get_tx_buffer_count = serial3TxCount, // .reset_write_buffer = serial3TxFlush, @@ -555,9 +431,9 @@ const io_stream_t* serial3Init(uint32_t baud_rate) .set_baud_rate = serial3SetBaudRate, .set_enqueue_rt_handler = serial3SetRtHandler }; - if (serial[0].flags.claimed) return NULL; + if (serial[2].flags.claimed) return NULL; - serial[0].flags.claimed = On; + serial[2].flags.claimed = On; GPIO_InitTypeDef GPIO_InitStructure = { 0 }; @@ -600,31 +476,279 @@ const io_stream_t* serial4Init(uint32_t baud_rate) void USART_GRBL_IRQHandler(void) { -// if (USART_GRBL->SR & USART_SR_RXNE) + if (USART_GRBL->SR & USART_SR_RXNE) + { + uint32_t data = USART_GRBL->DR; + if (!enqueue_realtime_command3((char) data)) + { // Check and strip realtime commands... + uint16_t next_head = BUFNEXT(rxbuf3.head, rxbuf3); // Get and increment buffer pointer + if (next_head == rxbuf3.tail) // If buffer full + rxbuf3.overflow = 1; // flag overflow + else + { + rxbuf3.data[rxbuf3.head] = (char) data; // if not add data to buffer + rxbuf3.head = next_head; // and update pointer + } + } + } + + if ((USART_GRBL->SR & USART_SR_TXE) && (USART_GRBL->CR1 & USART_CR1_TXEIE)) + { + uint_fast16_t tail = txbuf3.tail; // Get buffer pointer + USART_GRBL->DR = txbuf3.data[tail]; // Send next character + txbuf3.tail = tail = BUFNEXT(tail, txbuf3); // and increment pointer + if (tail == txbuf3.head) // If buffer empty then + USART_GRBL->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt + } +} +/**************************************************************************************************** + * + * + * + * + * + ****************************************************************************************************/ +//const io_stream_t *serial3Init (uint32_t baud_rate); +// +//static stream_rx_buffer_t rxbuf = +//{ 0 }; +//static stream_tx_buffer_t txbuf = +//{ 0 }; +//static enqueue_realtime_command_ptr enqueue_realtime_command = protocol_enqueue_realtime_command; +// +//#define USART USART3 +//#define USART_IRQHandler USART3_IRQHandler +// +//static io_stream_properties_t serial[] = +//{ +//{ +// .type = StreamType_Serial, +// .instance = 2, +// .flags.claimable = On, +// .flags.claimed = Off, +// .flags.connected = On, +// .flags.can_set_baud = On, +// .claim = serial3Init }, }; +// +//// +//// Returns number of free characters in serial input buffer +//// +//static uint16_t serialRxFree(void) +//{ +// uint16_t tail = rxbuf.tail, head = rxbuf.head; +// +// return RX_BUFFER_SIZE - BUFCOUNT(head, tail, RX_BUFFER_SIZE); +//} +// +//// +//// Flushes the serial input buffer +//// +//static void serialRxFlush(void) +//{ +// rxbuf.tail = rxbuf.head; +//} +// +//// +//// Flushes and adds a CAN character to the serial input buffer +//// +//static void serialRxCancel(void) +//{ +// rxbuf.data[rxbuf.head] = ASCII_CAN; +// rxbuf.tail = rxbuf.head; +// rxbuf.head = BUFNEXT(rxbuf.head, rxbuf); +//} +// +//// +//// Writes a character to the serial output stream +//// +//static bool serialPutC(const char c) +//{ +// uint16_t next_head = BUFNEXT(txbuf.head, txbuf); // Get pointer to next free slot in buffer +// +// while (txbuf.tail == next_head) +// { // While TX buffer full +// if (!hal.stream_blocking_callback()) // check if blocking for space, +// return false; // exit if not (leaves TX buffer in an inconsistent state) +// } +// txbuf.data[txbuf.head] = c; // Add data to buffer, +// txbuf.head = next_head; // update head pointer and +// USART->CR1 |= USART_CR1_TXEIE; // enable TX interrupts +// +// return true; +//} +// +//// +//// Writes a null terminated string to the serial output stream, blocks if buffer full +//// +//static void serialWriteS(const char *s) +//{ +// char c, *ptr = (char*) s; +// +// while ((c = *ptr++) != '\0') +// serialPutC(c); +//} +// +//// +//// Writes a number of characters from string to the serial output stream followed by EOL, blocks if buffer full +//// +//static void serialWrite(const char *s, uint16_t length) +//{ +// char *ptr = (char*) s; +// +// while (length--) +// serialPutC(*ptr++); +//} +// +//// +//// serialGetC - returns -1 if no data available +//// +//static int16_t serialGetC(void) +//{ +// uint_fast16_t tail = rxbuf.tail; // Get buffer pointer +// +// if (tail == rxbuf.head) return -1; // no data available +// +// char data = rxbuf.data[tail]; // Get next character +// rxbuf.tail = BUFNEXT(tail, rxbuf); // and update pointer +// +// return (int16_t) data; +//} +// +//static bool serialSuspendInput(bool suspend) +//{ +// return stream_rx_suspend(&rxbuf, suspend); +//} +// +//static bool serialSetBaudRate(uint32_t baud_rate) +//{ +// +// USART->CR1 = USART_CR1_RE | USART_CR1_TE; +// USART->BRR = UART_BRR_SAMPLING16(HAL_RCC_GetPCLK1Freq(), baud_rate); +// USART->CR1 |= (USART_CR1_UE | USART_CR1_RXNEIE); +// +// return true; +//} +// +//static bool serialDisable(bool disable) +//{ +// if (disable) +// USART->CR1 &= ~USART_CR1_RXNEIE; +// else +// USART->CR1 |= USART_CR1_RXNEIE; +// +// return true; +//} +// +//static bool serialEnqueueRtCommand(char c) +//{ +// return enqueue_realtime_command(c); +//} +// +//static enqueue_realtime_command_ptr serialSetRtHandler(enqueue_realtime_command_ptr handler) +//{ +// enqueue_realtime_command_ptr prev = enqueue_realtime_command; +// +// if (handler) enqueue_realtime_command = handler; +// +// return prev; +//} +// +//const io_stream_t* serial3Init(uint32_t baud_rate) +//{ +// static const io_stream_t stream = +// { +// .type = StreamType_Serial, +// .state.connected = On, +// .read = serialGetC, +// .write = serialWriteS, +// .write_n = serialWrite, +// .write_char = serialPutC, +// .enqueue_rt_command = serialEnqueueRtCommand, +// .get_rx_buffer_free = serialRxFree, +//// .get_rx_buffer_count = serialRxCount, +//// .get_tx_buffer_count = serialTxCount, +//// .reset_write_buffer = serialTxFlush, +// .reset_read_buffer = serialRxFlush, +// .cancel_read_buffer = serialRxCancel, +// .suspend_read = serialSuspendInput, +// .disable_rx = serialDisable, +// .set_baud_rate = serialSetBaudRate, +// .set_enqueue_rt_handler = serialSetRtHandler }; +// +// if (serial[2].flags.claimed) return NULL; +// +// serial[2].flags.claimed = On; +// +// GPIO_InitTypeDef GPIO_InitStructure = +// { 0 }; +// +// GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; +// GPIO_InitStructure.Pull = GPIO_NOPULL; +// GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_VERY_HIGH; +// +// __HAL_RCC_GPIOB_CLK_ENABLE(); +// __HAL_RCC_USART3_CLK_ENABLE(); +// +// GPIO_InitStructure.Pin = GPIO_PIN_10 | GPIO_PIN_11; +// GPIO_InitStructure.Alternate = GPIO_AF7_USART3; +// HAL_GPIO_Init(GPIOB, &GPIO_InitStructure); +// +// serialSetBaudRate(baud_rate); +// +// HAL_NVIC_SetPriority(USART3_IRQn, 0, 0); +// HAL_NVIC_EnableIRQ(USART3_IRQn); +// +// static const periph_pin_t tx = +// { .function = Output_TX, .group = PinGroup_UART, .port = GPIOB, .pin = 10, .mode = +// { .mask = PINMODE_OUTPUT }, .description = "UART3" }; +// +// static const periph_pin_t rx = +// { .function = Input_RX, .group = PinGroup_UART, .port = GPIOB, .pin = 11, .mode = +// { .mask = PINMODE_NONE }, .description = "UART3" }; +// +// hal.periph_port.register_pin(&rx); +// hal.periph_port.register_pin(&tx); +// +// return &stream; +//} +// +//void USART_IRQHandler(void) +//{ +// if (USART->SR & USART_SR_RXNE) // { -// uint32_t data = USART_GRBL->DR; -// if (!enqueue_realtime_command3((char) data)) +// uint32_t data = USART->DR; +// if (!enqueue_realtime_command((char) data)) // { // Check and strip realtime commands... -// uint16_t next_head = BUFNEXT(rxbuf3.head, rxbuf3); // Get and increment buffer pointer -// if (next_head == rxbuf3.tail) // If buffer full -// rxbuf3.overflow = 1; // flag overflow +// uint16_t next_head = BUFNEXT(rxbuf.head, rxbuf); // Get and increment buffer pointer +// if (next_head == rxbuf.tail) // If buffer full +// rxbuf.overflow = 1; // flag overflow // else // { -// rxbuf3.data[rxbuf3.head] = (char) data; // if not add data to buffer -// rxbuf3.head = next_head; // and update pointer +// rxbuf.data[rxbuf.head] = (char) data; // if not add data to buffer +// rxbuf.head = next_head; // and update pointer // } // } // } // -// if ((USART_GRBL->SR & USART_SR_TXE) && (USART_GRBL->CR1 & USART_CR1_TXEIE)) +// if ((USART->SR & USART_SR_TXE) && (USART->CR1 & USART_CR1_TXEIE)) // { -// uint_fast16_t tail = txbuf3.tail; // Get buffer pointer -// USART_GRBL->DR = txbuf3.data[tail]; // Send next character -// txbuf3.tail = tail = BUFNEXT(tail, txbuf3); // and increment pointer -// if (tail == txbuf3.head) // If buffer empty then -// USART_GRBL->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt +// uint_fast16_t tail = txbuf.tail; // Get buffer pointer +// USART->DR = txbuf.data[tail]; // Send next character +// txbuf.tail = tail = BUFNEXT(tail, txbuf); // and increment pointer +// if (tail == txbuf.head) // If buffer empty then +// USART->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt // } -} +//} +/* + * + * + * + * + * + * + * + * + */ /************************************ * From 1d383f690519626f2a5cd8823f3d6611bb770ea0 Mon Sep 17 00:00:00 2001 From: atlesg Date: Sat, 25 Dec 2021 14:00:22 +0700 Subject: [PATCH 05/10] Update Serial4Init, VFP_ENABLE is still not working --- .cproject | 5 +- Inc/my_machine.h | 33 ++-- Inc/my_machine_map.h | 8 +- Src/f4mpv5_hosoi.c | 445 ++++++++++++++++++++----------------------- 4 files changed, 229 insertions(+), 262 deletions(-) diff --git a/.cproject b/.cproject index 4fe96548..8c605d97 100644 --- a/.cproject +++ b/.cproject @@ -118,11 +118,12 @@ + - + - + diff --git a/Inc/my_machine.h b/Inc/my_machine.h index f8965872..b02be7b5 100644 --- a/Inc/my_machine.h +++ b/Inc/my_machine.h @@ -1,23 +1,23 @@ /* - my_machine.h - configuration for STM32F4xx ARM processors + my_machine.h - configuration for STM32F4xx ARM processors - Part of grblHAL + Part of grblHAL - Copyright (c) 2020-2021 Terje Io + Copyright (c) 2020-2021 Terje Io - Grbl 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. + Grbl 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. - Grbl 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. + Grbl 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 Grbl. If not, see . -*/ + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . + */ // NOTE: Only one board may be enabled! // If none is enabled pin mappings from generic_map.h will be used. @@ -52,9 +52,9 @@ //#define USB_SERIAL_CDC 1 // Serial communication via native USB. #endif //#define BLUETOOTH_ENABLE 1 // Set to 1 for HC-05 module. Requires and claims one auxillary input pin. -//#define VFD_ENABLE 1 // Set to 1 or 2 for Huanyang VFD spindle. More here https://github.com/grblHAL/Plugins_spindle +#define VFD_ENABLE 99 // Set to 1 or 2 for Huanyang VFD spindle. More here https://github.com/grblHAL/Plugins_spindle //#define DUAL_SPINDLE 1 // Uncomment for switching between VFD spindle and PWM output with $32 -//#define MODBUS_ENABLE 1 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin. +//#define MODBUS_ENABLE 2 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin. //#define SDCARD_ENABLE 2 // Run gcode programs from SD card. //#define KEYPAD_ENABLE 1 // Set to 1 for I2C keypad, 2 for other input such as serial data //#define ODOMETER_ENABLE 1 // Odometer plugin. @@ -83,4 +83,3 @@ //#define Y_GANGED_LIM_MAX 1 //#define Z_GANGED_LIM_MAX 1 // - diff --git a/Inc/my_machine_map.h b/Inc/my_machine_map.h index 6bc758b7..c55f2073 100644 --- a/Inc/my_machine_map.h +++ b/Inc/my_machine_map.h @@ -128,9 +128,13 @@ #define PROBE_PIN 12 // N15 // Define main grbl uart port -#define UART_INSTANCE 0 +#define UART_INSTANCE 2 + +// Define Spindle port and pins +#define MODBUS_SERIAL_PORT 3 +#define MODBUS_DIR_AUX 5 // Define I2C Port - avoid confliction with USART3 port -//#undef I2C_PORT +#undef I2C_PORT /* EOF */ diff --git a/Src/f4mpv5_hosoi.c b/Src/f4mpv5_hosoi.c index 1e8ad137..f3e469be 100644 --- a/Src/f4mpv5_hosoi.c +++ b/Src/f4mpv5_hosoi.c @@ -446,7 +446,7 @@ const io_stream_t* serial3Init(uint32_t baud_rate) __HAL_RCC_USART3_CLK_ENABLE(); GPIO_InitStructure.Pin = GPIO_PIN_10 | GPIO_PIN_11; - GPIO_InitStructure.Alternate = GPIO_AF7_USART1; + GPIO_InitStructure.Alternate = GPIO_AF7_USART3; HAL_GPIO_Init(GPIOB, &GPIO_InitStructure); serial3SetBaudRate(baud_rate); @@ -468,12 +468,6 @@ const io_stream_t* serial3Init(uint32_t baud_rate) return &stream; } -// SERIAL 4 -const io_stream_t* serial4Init(uint32_t baud_rate) -{ - -} - void USART_GRBL_IRQHandler(void) { if (USART_GRBL->SR & USART_SR_RXNE) @@ -501,254 +495,221 @@ void USART_GRBL_IRQHandler(void) USART_GRBL->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt } } -/**************************************************************************************************** - * - * - * - * - * - ****************************************************************************************************/ -//const io_stream_t *serial3Init (uint32_t baud_rate); -// -//static stream_rx_buffer_t rxbuf = -//{ 0 }; -//static stream_tx_buffer_t txbuf = -//{ 0 }; -//static enqueue_realtime_command_ptr enqueue_realtime_command = protocol_enqueue_realtime_command; -// -//#define USART USART3 -//#define USART_IRQHandler USART3_IRQHandler -// -//static io_stream_properties_t serial[] = -//{ -//{ -// .type = StreamType_Serial, -// .instance = 2, -// .flags.claimable = On, -// .flags.claimed = Off, -// .flags.connected = On, -// .flags.can_set_baud = On, -// .claim = serial3Init }, }; -// -//// -//// Returns number of free characters in serial input buffer -//// -//static uint16_t serialRxFree(void) -//{ -// uint16_t tail = rxbuf.tail, head = rxbuf.head; -// -// return RX_BUFFER_SIZE - BUFCOUNT(head, tail, RX_BUFFER_SIZE); -//} -// -//// -//// Flushes the serial input buffer -//// -//static void serialRxFlush(void) -//{ -// rxbuf.tail = rxbuf.head; -//} -// -//// -//// Flushes and adds a CAN character to the serial input buffer -//// -//static void serialRxCancel(void) -//{ -// rxbuf.data[rxbuf.head] = ASCII_CAN; -// rxbuf.tail = rxbuf.head; -// rxbuf.head = BUFNEXT(rxbuf.head, rxbuf); -//} -// -//// -//// Writes a character to the serial output stream -//// -//static bool serialPutC(const char c) -//{ -// uint16_t next_head = BUFNEXT(txbuf.head, txbuf); // Get pointer to next free slot in buffer -// -// while (txbuf.tail == next_head) -// { // While TX buffer full -// if (!hal.stream_blocking_callback()) // check if blocking for space, -// return false; // exit if not (leaves TX buffer in an inconsistent state) -// } -// txbuf.data[txbuf.head] = c; // Add data to buffer, -// txbuf.head = next_head; // update head pointer and -// USART->CR1 |= USART_CR1_TXEIE; // enable TX interrupts -// -// return true; -//} -// -//// -//// Writes a null terminated string to the serial output stream, blocks if buffer full -//// -//static void serialWriteS(const char *s) -//{ -// char c, *ptr = (char*) s; -// -// while ((c = *ptr++) != '\0') -// serialPutC(c); -//} -// -//// -//// Writes a number of characters from string to the serial output stream followed by EOL, blocks if buffer full -//// -//static void serialWrite(const char *s, uint16_t length) -//{ -// char *ptr = (char*) s; -// -// while (length--) -// serialPutC(*ptr++); -//} -// -//// -//// serialGetC - returns -1 if no data available -//// -//static int16_t serialGetC(void) -//{ -// uint_fast16_t tail = rxbuf.tail; // Get buffer pointer -// -// if (tail == rxbuf.head) return -1; // no data available -// -// char data = rxbuf.data[tail]; // Get next character -// rxbuf.tail = BUFNEXT(tail, rxbuf); // and update pointer -// -// return (int16_t) data; -//} -// -//static bool serialSuspendInput(bool suspend) -//{ -// return stream_rx_suspend(&rxbuf, suspend); -//} -// -//static bool serialSetBaudRate(uint32_t baud_rate) -//{ -// -// USART->CR1 = USART_CR1_RE | USART_CR1_TE; -// USART->BRR = UART_BRR_SAMPLING16(HAL_RCC_GetPCLK1Freq(), baud_rate); -// USART->CR1 |= (USART_CR1_UE | USART_CR1_RXNEIE); -// -// return true; -//} -// -//static bool serialDisable(bool disable) -//{ -// if (disable) -// USART->CR1 &= ~USART_CR1_RXNEIE; -// else -// USART->CR1 |= USART_CR1_RXNEIE; -// -// return true; -//} -// -//static bool serialEnqueueRtCommand(char c) -//{ -// return enqueue_realtime_command(c); -//} -// -//static enqueue_realtime_command_ptr serialSetRtHandler(enqueue_realtime_command_ptr handler) -//{ -// enqueue_realtime_command_ptr prev = enqueue_realtime_command; -// -// if (handler) enqueue_realtime_command = handler; -// -// return prev; -//} -// -//const io_stream_t* serial3Init(uint32_t baud_rate) -//{ -// static const io_stream_t stream = -// { -// .type = StreamType_Serial, -// .state.connected = On, -// .read = serialGetC, -// .write = serialWriteS, -// .write_n = serialWrite, -// .write_char = serialPutC, -// .enqueue_rt_command = serialEnqueueRtCommand, -// .get_rx_buffer_free = serialRxFree, -//// .get_rx_buffer_count = serialRxCount, -//// .get_tx_buffer_count = serialTxCount, -//// .reset_write_buffer = serialTxFlush, -// .reset_read_buffer = serialRxFlush, -// .cancel_read_buffer = serialRxCancel, -// .suspend_read = serialSuspendInput, -// .disable_rx = serialDisable, -// .set_baud_rate = serialSetBaudRate, -// .set_enqueue_rt_handler = serialSetRtHandler }; + +// SERIAL 4 +static stream_rx_buffer_t rxbuf4 = +{ 0 }; +static stream_tx_buffer_t txbuf4 = +{ 0 }; +static enqueue_realtime_command_ptr enqueue_realtime_command4 = protocol_enqueue_realtime_command; + +#define USART_MODBUS UART4 +#define USART_MODBUS_IRQHandler UART4_IRQHandler + + // -// if (serial[2].flags.claimed) return NULL; +// Returns number of free characters in serial input buffer // -// serial[2].flags.claimed = On; +static uint16_t serial4RxFree(void) +{ + uint16_t tail = rxbuf4.tail, head = rxbuf4.head; + + return RX_BUFFER_SIZE - BUFCOUNT(head, tail, RX_BUFFER_SIZE); +} + // -// GPIO_InitTypeDef GPIO_InitStructure = -// { 0 }; +// Flushes the serial input buffer // -// GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; -// GPIO_InitStructure.Pull = GPIO_NOPULL; -// GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_VERY_HIGH; +static void serial4RxFlush(void) +{ + rxbuf4.tail = rxbuf4.head; +} + // -// __HAL_RCC_GPIOB_CLK_ENABLE(); -// __HAL_RCC_USART3_CLK_ENABLE(); +// Flushes and adds a CAN character to the serial input buffer // -// GPIO_InitStructure.Pin = GPIO_PIN_10 | GPIO_PIN_11; -// GPIO_InitStructure.Alternate = GPIO_AF7_USART3; -// HAL_GPIO_Init(GPIOB, &GPIO_InitStructure); +static void serial4RxCancel(void) +{ + rxbuf4.data[rxbuf4.head] = ASCII_CAN; + rxbuf4.tail = rxbuf4.head; + rxbuf4.head = BUFNEXT(rxbuf4.head, rxbuf4); +} + // -// serialSetBaudRate(baud_rate); +// Writes a character to the serial output stream // -// HAL_NVIC_SetPriority(USART3_IRQn, 0, 0); -// HAL_NVIC_EnableIRQ(USART3_IRQn); +static bool serial4PutC(const char c) +{ + uint16_t next_head = BUFNEXT(txbuf4.head, txbuf4); // Get pointer to next free slot in buffer + + while (txbuf4.tail == next_head) + { // While TX buffer full + if (!hal.stream_blocking_callback()) // check if blocking for space, + return false; // exit if not (leaves TX buffer in an inconsistent state) + } + txbuf4.data[txbuf4.head] = c; // Add data to buffer, + txbuf4.head = next_head; // update head pointer and + USART_MODBUS->CR1 |= USART_CR1_TXEIE; // enable TX interrupts + + return true; +} + // -// static const periph_pin_t tx = -// { .function = Output_TX, .group = PinGroup_UART, .port = GPIOB, .pin = 10, .mode = -// { .mask = PINMODE_OUTPUT }, .description = "UART3" }; +// Writes a null terminated string to the serial output stream, blocks if buffer full // -// static const periph_pin_t rx = -// { .function = Input_RX, .group = PinGroup_UART, .port = GPIOB, .pin = 11, .mode = -// { .mask = PINMODE_NONE }, .description = "UART3" }; +static void serial4WriteS(const char *s) +{ + char c, *ptr = (char*) s; + + while ((c = *ptr++) != '\0') + serial4PutC(c); +} + // -// hal.periph_port.register_pin(&rx); -// hal.periph_port.register_pin(&tx); +// Writes a number of characters from string to the serial output stream followed by EOL, blocks if buffer full // -// return &stream; -//} +static void serial4Write(const char *s, uint16_t length) +{ + char *ptr = (char*) s; + + while (length--) + serial4PutC(*ptr++); +} + // -//void USART_IRQHandler(void) -//{ -// if (USART->SR & USART_SR_RXNE) -// { -// uint32_t data = USART->DR; -// if (!enqueue_realtime_command((char) data)) -// { // Check and strip realtime commands... -// uint16_t next_head = BUFNEXT(rxbuf.head, rxbuf); // Get and increment buffer pointer -// if (next_head == rxbuf.tail) // If buffer full -// rxbuf.overflow = 1; // flag overflow -// else -// { -// rxbuf.data[rxbuf.head] = (char) data; // if not add data to buffer -// rxbuf.head = next_head; // and update pointer -// } -// } -// } +// serialGetC - returns -1 if no data available // -// if ((USART->SR & USART_SR_TXE) && (USART->CR1 & USART_CR1_TXEIE)) -// { -// uint_fast16_t tail = txbuf.tail; // Get buffer pointer -// USART->DR = txbuf.data[tail]; // Send next character -// txbuf.tail = tail = BUFNEXT(tail, txbuf); // and increment pointer -// if (tail == txbuf.head) // If buffer empty then -// USART->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt -// } -//} -/* - * - * - * - * - * - * - * - * - */ +static int16_t serial4GetC(void) +{ + uint_fast16_t tail = rxbuf4.tail; // Get buffer pointer + + if (tail == rxbuf4.head) return -1; // no data available + + char data = rxbuf4.data[tail]; // Get next character + rxbuf4.tail = BUFNEXT(tail, rxbuf4); // and update pointer + + return (int16_t) data; +} + +static bool serial4SuspendInput(bool suspend) +{ + return stream_rx_suspend(&rxbuf4, suspend); +} + +static bool serial4SetBaudRate(uint32_t baud_rate) +{ +// if USART 1/6/9/10 → HAL_RCC_GetPCLK2Freq() - else: HAL_RCC_GetPCLK1Freq() + + USART_MODBUS->CR1 = USART_CR1_RE | USART_CR1_TE; + USART_MODBUS->BRR = UART_BRR_SAMPLING16(HAL_RCC_GetPCLK1Freq(), baud_rate); + USART_MODBUS->CR1 |= (USART_CR1_UE | USART_CR1_RXNEIE); + + return true; +} + +static bool serial4Disable(bool disable) +{ + if (disable) USART_MODBUS->CR1 &= ~USART_CR1_RXNEIE; + else USART_MODBUS->CR1 |= USART_CR1_RXNEIE; + + return true; +} + +static bool serial4EnqueueRtCommand(char c) +{ + return enqueue_realtime_command4(c); +} + +static enqueue_realtime_command_ptr serial4SetRtHandler(enqueue_realtime_command_ptr handler) +{ + enqueue_realtime_command_ptr prev = enqueue_realtime_command4; + + if (handler) enqueue_realtime_command4 = handler; + + return prev; +} + +const io_stream_t* serial4Init(uint32_t baud_rate) +{ + + static const io_stream_t stream = + { .type = StreamType_Serial, .state.connected = On, .read = serial4GetC, .write = serial4WriteS, .write_n = + serial4Write, .write_char = serial4PutC, .enqueue_rt_command = serial4EnqueueRtCommand, .get_rx_buffer_free = + serial4RxFree, +// .get_rx_buffer_count = serial4RxCount, +// .get_tx_buffer_count = serial4TxCount, +// .reset_write_buffer = serial4TxFlush, + .reset_read_buffer = serial4RxFlush, + .cancel_read_buffer = serial4RxCancel, + .suspend_read = serial4SuspendInput, + .disable_rx = serial4Disable, + .set_baud_rate = serial4SetBaudRate, + .set_enqueue_rt_handler = serial4SetRtHandler }; + + if (serial[3].flags.claimed) return NULL; + + serial[3].flags.claimed = On; + + GPIO_InitTypeDef GPIO_InitStructure = + { 0 }; + + GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; + GPIO_InitStructure.Pull = GPIO_NOPULL; + GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + + __HAL_RCC_GPIOC_CLK_ENABLE(); // USART4 Port + __HAL_RCC_UART4_CLK_ENABLE(); + + GPIO_InitStructure.Pin = GPIO_PIN_10 | GPIO_PIN_11; + GPIO_InitStructure.Alternate = GPIO_AF8_UART4; + HAL_GPIO_Init(GPIOC, &GPIO_InitStructure); + + serial4SetBaudRate(baud_rate); + + HAL_NVIC_SetPriority(UART4_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(UART4_IRQn); + + static const periph_pin_t tx = + { .function = Output_TX, .group = PinGroup_UART4, .port = GPIOC, .pin = 10, .mode = + { .mask = PINMODE_OUTPUT }, .description = "UART4" }; + + static const periph_pin_t rx = + { .function = Input_RX, .group = PinGroup_UART4, .port = GPIOC, .pin = 11, .mode = + { .mask = PINMODE_NONE }, .description = "UART4" }; + + hal.periph_port.register_pin(&rx); + hal.periph_port.register_pin(&tx); + + return &stream; +} + +void USART_MODBUS_IRQHandler(void) +{ + if (USART_MODBUS->SR & USART_SR_RXNE) + { + uint32_t data = USART_MODBUS->DR; + if (!enqueue_realtime_command4((char) data)) + { // Check and strip realtime commands... + uint16_t next_head = BUFNEXT(rxbuf4.head, rxbuf4); // Get and increment buffer pointer + if (next_head == rxbuf4.tail) // If buffer full + rxbuf4.overflow = 1; // flag overflow + else + { + rxbuf4.data[rxbuf4.head] = (char) data; // if not add data to buffer + rxbuf4.head = next_head; // and update pointer + } + } + } + + if ((USART_MODBUS->SR & USART_SR_TXE) && (USART_MODBUS->CR1 & USART_CR1_TXEIE)) + { + uint_fast16_t tail = txbuf4.tail; // Get buffer pointer + USART_MODBUS->DR = txbuf4.data[tail]; // Send next character + txbuf4.tail = tail = BUFNEXT(tail, txbuf4); // and increment pointer + if (tail == txbuf4.head) // If buffer empty then + USART_MODBUS->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt + } +} /************************************ * @@ -776,6 +737,8 @@ void board_init(void) static io_stream_details_t streams = { .n_streams = sizeof(serial) / sizeof(io_stream_properties_t), .streams = serial, }; stream_register_streams(&streams); +// Enable VFD Spindle Control + // Enable Encoder Timer // MX_TIM2_Init(); From 50eda78592effc132b1b8c377347287eb46e7f46 Mon Sep 17 00:00:00 2001 From: atlesg Date: Sat, 25 Dec 2021 17:11:23 +0700 Subject: [PATCH 06/10] Updated modbus to work - Hardcoded UART4 init in f4mpv5_hosoi.c + baurate = 9600 + parity = odd --- Inc/my_machine.h | 4 +- Inc/my_machine_map.h | 2 +- Src/f4mpv5_hosoi.c | 554 +++++++++++++++++++++++++++---------------- 3 files changed, 348 insertions(+), 212 deletions(-) diff --git a/Inc/my_machine.h b/Inc/my_machine.h index b02be7b5..f04942ff 100644 --- a/Inc/my_machine.h +++ b/Inc/my_machine.h @@ -52,9 +52,9 @@ //#define USB_SERIAL_CDC 1 // Serial communication via native USB. #endif //#define BLUETOOTH_ENABLE 1 // Set to 1 for HC-05 module. Requires and claims one auxillary input pin. -#define VFD_ENABLE 99 // Set to 1 or 2 for Huanyang VFD spindle. More here https://github.com/grblHAL/Plugins_spindle +#define VFD_ENABLE 100 // Set to 1 or 2 for Huanyang VFD spindle. More here https://github.com/grblHAL/Plugins_spindle //#define DUAL_SPINDLE 1 // Uncomment for switching between VFD spindle and PWM output with $32 -//#define MODBUS_ENABLE 2 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin. +#define MODBUS_ENABLE 1 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin, 3 for shihlin vfd (author:tai) //#define SDCARD_ENABLE 2 // Run gcode programs from SD card. //#define KEYPAD_ENABLE 1 // Set to 1 for I2C keypad, 2 for other input such as serial data //#define ODOMETER_ENABLE 1 // Odometer plugin. diff --git a/Inc/my_machine_map.h b/Inc/my_machine_map.h index c55f2073..a4871791 100644 --- a/Inc/my_machine_map.h +++ b/Inc/my_machine_map.h @@ -132,7 +132,7 @@ // Define Spindle port and pins #define MODBUS_SERIAL_PORT 3 -#define MODBUS_DIR_AUX 5 +//#define MODBUS_DIR_AUX 5 // Define I2C Port - avoid confliction with USART3 port #undef I2C_PORT diff --git a/Src/f4mpv5_hosoi.c b/Src/f4mpv5_hosoi.c index f3e469be..f3067598 100644 --- a/Src/f4mpv5_hosoi.c +++ b/Src/f4mpv5_hosoi.c @@ -21,12 +21,15 @@ #include "grbl/stepdir_map.h" TIM_HandleTypeDef htim2; +UART_HandleTypeDef huart4; static on_report_options_ptr on_report_options; static on_execute_realtime_ptr on_execute_realtime; stepper_t stepper = -{ 0 }; + { + 0 + }; static bool encoder_interrupt_busy = false; @@ -35,19 +38,20 @@ static void encoder_handler(sys_state_t state); // Add info about our plugin to the $I report. static void on_report_my_options(bool newopt) -{ + { on_report_options(newopt); - if (!newopt) hal.stream.write("[PLUGIN:Blink LED v1.00]" ASCII_EOL); -} + if (!newopt) + hal.stream.write("[PLUGIN:Blink LED v1.00]" ASCII_EOL); + } static void blink_led(sys_state_t state) -{ + { // static bool led_on = false; static uint32_t ms = 0; if (hal.get_elapsed_ticks() >= ms) - { + { ms = hal.get_elapsed_ticks() + 1000; //ms // HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_6); // Green @@ -68,10 +72,10 @@ static void blink_led(sys_state_t state) // GPIOC->ODR &= ~GPIO_PIN_13; // } - } + } on_execute_realtime(state); -} + } //#define SLOW_JOG #define FAST_JOG @@ -83,7 +87,7 @@ volatile int16_t prev_count = 0; volatile int16_t delta_count = 0; void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) -{ + { /* Prevent unused argument(s) compilation warning */ UNUSED(htim); @@ -91,14 +95,18 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) counter = __HAL_TIM_GET_COUNTER(htim); count = (int16_t) (counter / 4); delta_count = count - prev_count; -} + } static void MX_TIM2_Init(void) -{ + { TIM_Encoder_InitTypeDef sConfig = - { 0 }; + { + 0 + }; TIM_MasterConfigTypeDef sMasterConfig = - { 0 }; + { + 0 + }; htim2.Instance = TIM2; htim2.Init.Prescaler = 0; htim2.Init.CounterMode = TIM_COUNTERMODE_UP; @@ -115,19 +123,19 @@ static void MX_TIM2_Init(void) sConfig.IC2Prescaler = TIM_ICPSC_DIV1; sConfig.IC2Filter = 0; if (HAL_TIM_Encoder_Init(&htim2, &sConfig) != HAL_OK) - { + { Error_Handler(); - } + } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) - { + { Error_Handler(); + } } -} void led_init(void) -{ + { GPIO_InitTypeDef GPIO_InitStruct; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; @@ -137,11 +145,12 @@ void led_init(void) GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8, GPIO_PIN_RESET); -} + HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8, + GPIO_PIN_RESET); + } void output_init(void) -{ + { GPIO_InitTypeDef GPIO_InitStruct; @@ -168,20 +177,21 @@ void output_init(void) ////KEY // GPIO_InitStruct.Pin = GPIO_PIN_12; // HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); -} + } static void encoder_handler(sys_state_t state) -{ + { - if (encoder_interrupt_busy == true) return; + if (encoder_interrupt_busy == true) + return; if ((delta_count < 0) || (delta_count > 0)) - { + { encoder_interrupt_busy = true; - } + } if (delta_count > 0) - { + { #ifdef SLOW_JOG @@ -195,7 +205,7 @@ static void encoder_handler(sys_state_t state) #endif #ifdef FAST_JOG for (uint16_t step = 0; step < 100; step++) - { + { stepper.step_outbits.x = 1; stepper.dir_outbits.x = 1; stepper.dir_change = 1; // set when dir_outbits is changed. @@ -206,7 +216,7 @@ static void encoder_handler(sys_state_t state) hal.delay_ms(1, NULL); - } + } // stepper.step_outbits.x = 1; // stepper.dir_outbits.x = 1; // stepper.dir_change = 1; // set when dir_outbits is changed. @@ -216,9 +226,9 @@ static void encoder_handler(sys_state_t state) #endif - } + } else if (delta_count < 0) - { + { #ifdef SLOW_JOG stepper.step_outbits.x = 1; @@ -230,7 +240,7 @@ static void encoder_handler(sys_state_t state) #endif #ifdef FAST_JOG for (uint16_t step = 0; step < 100; step++) - { + { stepper.step_outbits.x = 1; stepper.dir_outbits.x = 0; stepper.dir_change = 1; // set when dir_outbits is changed. @@ -241,14 +251,14 @@ static void encoder_handler(sys_state_t state) hal.delay_ms(1, NULL); - } + } #endif - } + } prev_count = count; encoder_interrupt_busy = false; -} + } /************************************ * @@ -259,130 +269,138 @@ const io_stream_t* serial3Init(uint32_t baud_rate); const io_stream_t* serial4Init(uint32_t baud_rate); static stream_rx_buffer_t rxbuf3 = -{ 0 }; + { + 0 + }; static stream_tx_buffer_t txbuf3 = -{ 0 }; -static enqueue_realtime_command_ptr enqueue_realtime_command3 = protocol_enqueue_realtime_command; + { + 0 + }; +static enqueue_realtime_command_ptr enqueue_realtime_command3 = + protocol_enqueue_realtime_command; #define USART_GRBL USART3 #define USART_GRBL_IRQHandler USART3_IRQHandler static io_stream_properties_t serial[] = -{ -{ - .type = StreamType_Serial, - .instance = 2, - .flags.claimable = On, - .flags.claimed = Off, - .flags.connected = On, - .flags.can_set_baud = - On, - .claim = serial3Init }, - -{ - .type = StreamType_Serial, - .instance = 3, - .flags.claimable = On, - .flags.claimed = Off, - .flags.connected = On, - .flags.can_set_baud = - On, - .flags.modbus_ready = On, - .claim = serial4Init } - -}; + { + { + .type = StreamType_Serial, + .instance = 2, + .flags.claimable = On, + .flags.claimed = Off, + .flags.connected = On, + .flags.can_set_baud = + On, + .claim = serial3Init + }, + + { + .type = StreamType_Serial, + .instance = 3, + .flags.claimable = On, + .flags.claimed = Off, + .flags.connected = On, + .flags.can_set_baud = + On, + .flags.modbus_ready = On, + .claim = serial4Init + } + + }; // // Returns number of free characters in serial input buffer // static uint16_t serial3RxFree(void) -{ + { uint16_t tail = rxbuf3.tail, head = rxbuf3.head; return RX_BUFFER_SIZE - BUFCOUNT(head, tail, RX_BUFFER_SIZE); -} + } // // Flushes the serial input buffer // static void serial3RxFlush(void) -{ + { rxbuf3.tail = rxbuf3.head; -} + } // // Flushes and adds a CAN character to the serial input buffer // static void serial3RxCancel(void) -{ + { rxbuf3.data[rxbuf3.head] = ASCII_CAN; rxbuf3.tail = rxbuf3.head; rxbuf3.head = BUFNEXT(rxbuf3.head, rxbuf3); -} + } // // Writes a character to the serial output stream // static bool serial3PutC(const char c) -{ - uint16_t next_head = BUFNEXT(txbuf3.head, txbuf3); // Get pointer to next free slot in buffer + { + uint16_t next_head = BUFNEXT(txbuf3.head, txbuf3); // Get pointer to next free slot in buffer while (txbuf3.tail == next_head) - { // While TX buffer full - if (!hal.stream_blocking_callback()) // check if blocking for space, - return false; // exit if not (leaves TX buffer in an inconsistent state) - } - txbuf3.data[txbuf3.head] = c; // Add data to buffer, - txbuf3.head = next_head; // update head pointer and - USART_GRBL->CR1 |= USART_CR1_TXEIE; // enable TX interrupts + { // While TX buffer full + if (!hal.stream_blocking_callback()) // check if blocking for space, + return false; // exit if not (leaves TX buffer in an inconsistent state) + } + txbuf3.data[txbuf3.head] = c; // Add data to buffer, + txbuf3.head = next_head; // update head pointer and + USART_GRBL->CR1 |= USART_CR1_TXEIE; // enable TX interrupts return true; -} + } // // Writes a null terminated string to the serial output stream, blocks if buffer full // static void serial3WriteS(const char *s) -{ + { char c, *ptr = (char*) s; while ((c = *ptr++) != '\0') serial3PutC(c); -} + } // // Writes a number of characters from string to the serial output stream followed by EOL, blocks if buffer full // static void serial3Write(const char *s, uint16_t length) -{ + { char *ptr = (char*) s; while (length--) serial3PutC(*ptr++); -} + } // // serialGetC - returns -1 if no data available // static int16_t serial3GetC(void) -{ + { uint_fast16_t tail = rxbuf3.tail; // Get buffer pointer - if (tail == rxbuf3.head) return -1; // no data available + if (tail == rxbuf3.head) + return -1; // no data available char data = rxbuf3.data[tail]; // Get next character rxbuf3.tail = BUFNEXT(tail, rxbuf3); // and update pointer return (int16_t) data; -} + } static bool serial3SuspendInput(bool suspend) -{ + { return stream_rx_suspend(&rxbuf3, suspend); -} + } static bool serial3SetBaudRate(uint32_t baud_rate) -{ + { // if USART 1/6/9/10 → HAL_RCC_GetPCLK2Freq() - else: HAL_RCC_GetPCLK1Freq() USART_GRBL->CR1 = USART_CR1_RE | USART_CR1_TE; @@ -390,53 +408,69 @@ static bool serial3SetBaudRate(uint32_t baud_rate) USART_GRBL->CR1 |= (USART_CR1_UE | USART_CR1_RXNEIE); return true; -} + } static bool serial3Disable(bool disable) -{ - if (disable) USART_GRBL->CR1 &= ~USART_CR1_RXNEIE; - else USART_GRBL->CR1 |= USART_CR1_RXNEIE; + { + if (disable) + USART_GRBL->CR1 &= ~USART_CR1_RXNEIE; + else + USART_GRBL->CR1 |= USART_CR1_RXNEIE; return true; -} + } static bool serial3EnqueueRtCommand(char c) -{ + { return enqueue_realtime_command3(c); -} + } -static enqueue_realtime_command_ptr serial3SetRtHandler(enqueue_realtime_command_ptr handler) -{ +static enqueue_realtime_command_ptr serial3SetRtHandler( + enqueue_realtime_command_ptr handler) + { enqueue_realtime_command_ptr prev = enqueue_realtime_command3; - if (handler) enqueue_realtime_command3 = handler; + if (handler) + enqueue_realtime_command3 = handler; return prev; -} + } const io_stream_t* serial3Init(uint32_t baud_rate) -{ + { static const io_stream_t stream = - { .type = StreamType_Serial, .state.connected = On, .read = serial3GetC, .write = serial3WriteS, .write_n = - serial3Write, .write_char = serial3PutC, .enqueue_rt_command = serial3EnqueueRtCommand, .get_rx_buffer_free = - serial3RxFree, -// .get_rx_buffer_count = serial3RxCount, + { + .type = StreamType_Serial, + .state.connected = On, + .read = serial3GetC, + .write = serial3WriteS, + .write_n = + serial3Write, + .write_char = serial3PutC, + .enqueue_rt_command = serial3EnqueueRtCommand, + .get_rx_buffer_free = + serial3RxFree, + // .get_rx_buffer_count = serial3RxCount, // .get_tx_buffer_count = serial3TxCount, // .reset_write_buffer = serial3TxFlush, - .reset_read_buffer = serial3RxFlush, - .cancel_read_buffer = serial3RxCancel, - .suspend_read = serial3SuspendInput, - .disable_rx = serial3Disable, - .set_baud_rate = serial3SetBaudRate, - .set_enqueue_rt_handler = serial3SetRtHandler }; + .reset_read_buffer = serial3RxFlush, + .cancel_read_buffer = serial3RxCancel, + .suspend_read = serial3SuspendInput, + .disable_rx = serial3Disable, + .set_baud_rate = serial3SetBaudRate, + .set_enqueue_rt_handler = serial3SetRtHandler + }; - if (serial[2].flags.claimed) return NULL; + if (serial[0].flags.claimed) + return NULL; - serial[2].flags.claimed = On; + serial[0].flags.claimed = On; GPIO_InitTypeDef GPIO_InitStructure = - { 0 }; + { + 0 + }; GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; GPIO_InitStructure.Pull = GPIO_NOPULL; @@ -455,203 +489,284 @@ const io_stream_t* serial3Init(uint32_t baud_rate) HAL_NVIC_EnableIRQ(USART3_IRQn); static const periph_pin_t tx = - { .function = Output_TX, .group = PinGroup_UART3, .port = GPIOB, .pin = 10, .mode = - { .mask = PINMODE_OUTPUT }, .description = "UART3" }; + { + .function = Output_TX, + .group = PinGroup_UART3, + .port = GPIOB, + .pin = 10, + .mode = + { + .mask = PINMODE_OUTPUT + }, + .description = "UART3" + }; static const periph_pin_t rx = - { .function = Input_RX, .group = PinGroup_UART3, .port = GPIOB, .pin = 11, .mode = - { .mask = PINMODE_NONE }, .description = "UART3" }; + { + .function = Input_RX, + .group = PinGroup_UART3, + .port = GPIOB, + .pin = 11, + .mode = + { + .mask = PINMODE_NONE + }, + .description = "UART3" + }; hal.periph_port.register_pin(&rx); hal.periph_port.register_pin(&tx); return &stream; -} + } void USART_GRBL_IRQHandler(void) -{ - if (USART_GRBL->SR & USART_SR_RXNE) { + if (USART_GRBL->SR & USART_SR_RXNE) + { uint32_t data = USART_GRBL->DR; if (!enqueue_realtime_command3((char) data)) - { // Check and strip realtime commands... - uint16_t next_head = BUFNEXT(rxbuf3.head, rxbuf3); // Get and increment buffer pointer + { // Check and strip realtime commands... + uint16_t next_head = BUFNEXT(rxbuf3.head, rxbuf3); // Get and increment buffer pointer if (next_head == rxbuf3.tail) // If buffer full - rxbuf3.overflow = 1; // flag overflow + rxbuf3.overflow = 1; // flag overflow else - { - rxbuf3.data[rxbuf3.head] = (char) data; // if not add data to buffer + { + rxbuf3.data[rxbuf3.head] = (char) data; // if not add data to buffer rxbuf3.head = next_head; // and update pointer + } } } - } if ((USART_GRBL->SR & USART_SR_TXE) && (USART_GRBL->CR1 & USART_CR1_TXEIE)) - { + { uint_fast16_t tail = txbuf3.tail; // Get buffer pointer USART_GRBL->DR = txbuf3.data[tail]; // Send next character txbuf3.tail = tail = BUFNEXT(tail, txbuf3); // and increment pointer if (tail == txbuf3.head) // If buffer empty then - USART_GRBL->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt + USART_GRBL->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt + } } -} // SERIAL 4 static stream_rx_buffer_t rxbuf4 = -{ 0 }; + { + 0 + }; static stream_tx_buffer_t txbuf4 = -{ 0 }; -static enqueue_realtime_command_ptr enqueue_realtime_command4 = protocol_enqueue_realtime_command; + { + 0 + }; +static enqueue_realtime_command_ptr enqueue_realtime_command4 = + protocol_enqueue_realtime_command; #define USART_MODBUS UART4 #define USART_MODBUS_IRQHandler UART4_IRQHandler - // // Returns number of free characters in serial input buffer // static uint16_t serial4RxFree(void) -{ + { uint16_t tail = rxbuf4.tail, head = rxbuf4.head; return RX_BUFFER_SIZE - BUFCOUNT(head, tail, RX_BUFFER_SIZE); -} + } + +// +// Returns number of characters in serial input buffer +// +static uint16_t serial4RxCount(void) + { + uint32_t tail = rxbuf4.tail, head = rxbuf4.head; + + return BUFCOUNT(head, tail, RX_BUFFER_SIZE); + } // // Flushes the serial input buffer // static void serial4RxFlush(void) -{ + { rxbuf4.tail = rxbuf4.head; -} + } // // Flushes and adds a CAN character to the serial input buffer // static void serial4RxCancel(void) -{ + { rxbuf4.data[rxbuf4.head] = ASCII_CAN; rxbuf4.tail = rxbuf4.head; rxbuf4.head = BUFNEXT(rxbuf4.head, rxbuf4); -} + } // // Writes a character to the serial output stream // static bool serial4PutC(const char c) -{ - uint16_t next_head = BUFNEXT(txbuf4.head, txbuf4); // Get pointer to next free slot in buffer + { + uint16_t next_head = BUFNEXT(txbuf4.head, txbuf4); // Get pointer to next free slot in buffer while (txbuf4.tail == next_head) - { // While TX buffer full - if (!hal.stream_blocking_callback()) // check if blocking for space, - return false; // exit if not (leaves TX buffer in an inconsistent state) - } - txbuf4.data[txbuf4.head] = c; // Add data to buffer, - txbuf4.head = next_head; // update head pointer and - USART_MODBUS->CR1 |= USART_CR1_TXEIE; // enable TX interrupts + { // While TX buffer full + if (!hal.stream_blocking_callback()) // check if blocking for space, + return false; // exit if not (leaves TX buffer in an inconsistent state) + } + txbuf4.data[txbuf4.head] = c; // Add data to buffer, + txbuf4.head = next_head; // update head pointer and + USART_MODBUS->CR1 |= USART_CR1_TXEIE; // enable TX interrupts return true; -} + } // // Writes a null terminated string to the serial output stream, blocks if buffer full // static void serial4WriteS(const char *s) -{ + { char c, *ptr = (char*) s; while ((c = *ptr++) != '\0') serial4PutC(c); -} + } // // Writes a number of characters from string to the serial output stream followed by EOL, blocks if buffer full // static void serial4Write(const char *s, uint16_t length) -{ + { char *ptr = (char*) s; while (length--) serial4PutC(*ptr++); -} + } + +// +// Flushes the serial output buffer +// +static void serial4TxFlush(void) + { + UART4->CR1 &= ~USART_CR1_TXEIE; // Disable TX interrupts + txbuf4.tail = txbuf4.head; + } + +// +// Returns number of characters pending transmission +// +static uint16_t serial4TxCount(void) + { + uint32_t tail = txbuf4.tail, head = txbuf4.head; + + return BUFCOUNT(head, tail, TX_BUFFER_SIZE) + + (UART4->SR & USART_SR_TC ? 0 : 1); + } // // serialGetC - returns -1 if no data available // static int16_t serial4GetC(void) -{ + { uint_fast16_t tail = rxbuf4.tail; // Get buffer pointer - if (tail == rxbuf4.head) return -1; // no data available + if (tail == rxbuf4.head) + return -1; // no data available char data = rxbuf4.data[tail]; // Get next character rxbuf4.tail = BUFNEXT(tail, rxbuf4); // and update pointer return (int16_t) data; -} + } static bool serial4SuspendInput(bool suspend) -{ + { return stream_rx_suspend(&rxbuf4, suspend); -} + } static bool serial4SetBaudRate(uint32_t baud_rate) -{ + { // if USART 1/6/9/10 → HAL_RCC_GetPCLK2Freq() - else: HAL_RCC_GetPCLK1Freq() USART_MODBUS->CR1 = USART_CR1_RE | USART_CR1_TE; - USART_MODBUS->BRR = UART_BRR_SAMPLING16(HAL_RCC_GetPCLK1Freq(), baud_rate); + USART_MODBUS->BRR = UART_BRR_SAMPLING16(HAL_RCC_GetPCLK1Freq(), 38400); USART_MODBUS->CR1 |= (USART_CR1_UE | USART_CR1_RXNEIE); + huart4.Instance = UART4; + huart4.Init.BaudRate = 38400; + huart4.Init.WordLength = UART_WORDLENGTH_9B; + huart4.Init.StopBits = UART_STOPBITS_1; + huart4.Init.Parity = UART_PARITY_EVEN; + huart4.Init.Mode = UART_MODE_TX_RX; + huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart4.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart4) != HAL_OK) + { + Error_Handler(); + } + return true; -} + } static bool serial4Disable(bool disable) -{ - if (disable) USART_MODBUS->CR1 &= ~USART_CR1_RXNEIE; - else USART_MODBUS->CR1 |= USART_CR1_RXNEIE; + { + if (disable) + USART_MODBUS->CR1 &= ~USART_CR1_RXNEIE; + else + USART_MODBUS->CR1 |= USART_CR1_RXNEIE; return true; -} + } static bool serial4EnqueueRtCommand(char c) -{ + { return enqueue_realtime_command4(c); -} + } -static enqueue_realtime_command_ptr serial4SetRtHandler(enqueue_realtime_command_ptr handler) -{ +static enqueue_realtime_command_ptr serial4SetRtHandler( + enqueue_realtime_command_ptr handler) + { enqueue_realtime_command_ptr prev = enqueue_realtime_command4; - if (handler) enqueue_realtime_command4 = handler; + if (handler) + enqueue_realtime_command4 = handler; return prev; -} + } const io_stream_t* serial4Init(uint32_t baud_rate) -{ + { static const io_stream_t stream = - { .type = StreamType_Serial, .state.connected = On, .read = serial4GetC, .write = serial4WriteS, .write_n = - serial4Write, .write_char = serial4PutC, .enqueue_rt_command = serial4EnqueueRtCommand, .get_rx_buffer_free = - serial4RxFree, -// .get_rx_buffer_count = serial4RxCount, -// .get_tx_buffer_count = serial4TxCount, -// .reset_write_buffer = serial4TxFlush, - .reset_read_buffer = serial4RxFlush, - .cancel_read_buffer = serial4RxCancel, - .suspend_read = serial4SuspendInput, - .disable_rx = serial4Disable, - .set_baud_rate = serial4SetBaudRate, - .set_enqueue_rt_handler = serial4SetRtHandler }; - - if (serial[3].flags.claimed) return NULL; - - serial[3].flags.claimed = On; + { + .type = StreamType_Serial, + .instance = 3, + .state.connected = On, + .read = serial4GetC, + .write = serial4WriteS, + .write_n = serial4Write, + .write_char = serial4PutC, + .enqueue_rt_command = serial4EnqueueRtCommand, + .get_rx_buffer_free = serial4RxFree, + .get_rx_buffer_count = serial4RxCount, + .get_tx_buffer_count = serial4TxCount, + .reset_write_buffer = serial4TxFlush, + .reset_read_buffer = serial4RxFlush, + .cancel_read_buffer = serial4RxCancel, + .suspend_read = serial4SuspendInput, + .disable_rx = serial4Disable, + .set_baud_rate = serial4SetBaudRate, + .set_enqueue_rt_handler = serial4SetRtHandler + }; + + if (serial[1].flags.claimed) + return NULL; + + serial[1].flags.claimed = On; GPIO_InitTypeDef GPIO_InitStructure = - { 0 }; + { + 0 + }; GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; GPIO_InitStructure.Pull = GPIO_NOPULL; @@ -670,46 +785,65 @@ const io_stream_t* serial4Init(uint32_t baud_rate) HAL_NVIC_EnableIRQ(UART4_IRQn); static const periph_pin_t tx = - { .function = Output_TX, .group = PinGroup_UART4, .port = GPIOC, .pin = 10, .mode = - { .mask = PINMODE_OUTPUT }, .description = "UART4" }; + { + .function = Output_TX, + .group = PinGroup_UART4, + .port = GPIOC, + .pin = 10, + .mode = + { + .mask = PINMODE_OUTPUT + }, + .description = "UART4" + }; static const periph_pin_t rx = - { .function = Input_RX, .group = PinGroup_UART4, .port = GPIOC, .pin = 11, .mode = - { .mask = PINMODE_NONE }, .description = "UART4" }; + { + .function = Input_RX, + .group = PinGroup_UART4, + .port = GPIOC, + .pin = 11, + .mode = + { + .mask = PINMODE_NONE + }, + .description = "UART4" + }; hal.periph_port.register_pin(&rx); hal.periph_port.register_pin(&tx); return &stream; -} + } void USART_MODBUS_IRQHandler(void) -{ - if (USART_MODBUS->SR & USART_SR_RXNE) { + if (USART_MODBUS->SR & USART_SR_RXNE) + { uint32_t data = USART_MODBUS->DR; if (!enqueue_realtime_command4((char) data)) - { // Check and strip realtime commands... - uint16_t next_head = BUFNEXT(rxbuf4.head, rxbuf4); // Get and increment buffer pointer + { // Check and strip realtime commands... + uint16_t next_head = BUFNEXT(rxbuf4.head, rxbuf4); // Get and increment buffer pointer if (next_head == rxbuf4.tail) // If buffer full - rxbuf4.overflow = 1; // flag overflow + rxbuf4.overflow = 1; // flag overflow else - { - rxbuf4.data[rxbuf4.head] = (char) data; // if not add data to buffer + { + rxbuf4.data[rxbuf4.head] = (char) data; // if not add data to buffer rxbuf4.head = next_head; // and update pointer + } } } - } - if ((USART_MODBUS->SR & USART_SR_TXE) && (USART_MODBUS->CR1 & USART_CR1_TXEIE)) - { + if ((USART_MODBUS->SR & USART_SR_TXE) + && (USART_MODBUS->CR1 & USART_CR1_TXEIE)) + { uint_fast16_t tail = txbuf4.tail; // Get buffer pointer USART_MODBUS->DR = txbuf4.data[tail]; // Send next character txbuf4.tail = tail = BUFNEXT(tail, txbuf4); // and increment pointer if (tail == txbuf4.head) // If buffer empty then - USART_MODBUS->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt + USART_MODBUS->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt + } } -} /************************************ * @@ -717,7 +851,7 @@ void USART_MODBUS_IRQHandler(void) * ************************************/ void board_init(void) -{ + { // Add info about our plugin to the $I report. on_report_options = grbl.on_report_options; grbl.on_report_options = on_report_my_options; @@ -735,14 +869,16 @@ void board_init(void) // Enable UART static io_stream_details_t streams = - { .n_streams = sizeof(serial) / sizeof(io_stream_properties_t), .streams = serial, }; + { + .n_streams = sizeof(serial) / sizeof(io_stream_properties_t), .streams = + serial, + }; stream_register_streams(&streams); // Enable VFD Spindle Control - // Enable Encoder Timer // MX_TIM2_Init(); // HAL_TIM_Encoder_Start_IT(&htim2, TIM_CHANNEL_ALL); -} + } From be5c3bd4ded5129bb2fc8879231ca6b31f411fa6 Mon Sep 17 00:00:00 2001 From: atlesg Date: Tue, 28 Dec 2021 10:03:22 +0700 Subject: [PATCH 07/10] Work in Progress - TODO: Enable toggling spindle state using auxiliary pins --- Inc/my_machine.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Inc/my_machine.h b/Inc/my_machine.h index f04942ff..34b00205 100644 --- a/Inc/my_machine.h +++ b/Inc/my_machine.h @@ -52,9 +52,10 @@ //#define USB_SERIAL_CDC 1 // Serial communication via native USB. #endif //#define BLUETOOTH_ENABLE 1 // Set to 1 for HC-05 module. Requires and claims one auxillary input pin. -#define VFD_ENABLE 100 // Set to 1 or 2 for Huanyang VFD spindle. More here https://github.com/grblHAL/Plugins_spindle +#define VFD_ENABLE 101 // Set to 1 or 2 for Huanyang VFD spindle. More here https://github.com/grblHAL/Plugins_spindle +//#define SHIHLIN_ENABLE 1 //#define DUAL_SPINDLE 1 // Uncomment for switching between VFD spindle and PWM output with $32 -#define MODBUS_ENABLE 1 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin, 3 for shihlin vfd (author:tai) +#define MODBUS_ENABLE 3 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin, 3 for shihlin vfd (author:tai) //#define SDCARD_ENABLE 2 // Run gcode programs from SD card. //#define KEYPAD_ENABLE 1 // Set to 1 for I2C keypad, 2 for other input such as serial data //#define ODOMETER_ENABLE 1 // Odometer plugin. From c9afc0eef3be8b0a7aec6afab996a5d890c7384e Mon Sep 17 00:00:00 2001 From: atlesg Date: Thu, 20 Jan 2022 15:22:18 +0700 Subject: [PATCH 08/10] Add 3 encoders, there are still some problems.. --- .settings/language.settings.xml | 36 +- .vscode/launch.json | 6 +- Inc/my_machine.h | 6 +- Src/f4mpv4_hosoi.h | 17 + Src/f4mpv5_hosoi.c | 1362 ++++++++++++++++++------------- Src/stm32f4xx_hal_msp.c | 57 +- 6 files changed, 842 insertions(+), 642 deletions(-) diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index eec2ee70..ad47745a 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -6,7 +6,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -30,7 +30,7 @@ - + @@ -42,7 +42,7 @@ - + @@ -54,7 +54,7 @@ - + @@ -66,7 +66,7 @@ - + @@ -78,7 +78,7 @@ - + @@ -90,7 +90,7 @@ - + @@ -102,7 +102,7 @@ - + @@ -114,7 +114,7 @@ - + @@ -126,7 +126,7 @@ - + @@ -138,7 +138,7 @@ - + @@ -150,7 +150,7 @@ - + @@ -162,7 +162,7 @@ - + @@ -174,7 +174,7 @@ - + @@ -186,7 +186,7 @@ - + @@ -198,7 +198,7 @@ - + @@ -210,7 +210,7 @@ - + diff --git a/.vscode/launch.json b/.vscode/launch.json index 45ae99f6..7a2f2f09 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -14,7 +14,7 @@ "name": "PIO Debug", "executable": "e:/TAI/01_AKB/STM32CubeIDE/STM32F4xx/.pio/build/btt_skr_pro_1_1/firmware.elf", "projectEnvName": "btt_skr_pro_1_1", - "toolchainBinDir": "C:/Users/tai/.platformio/packages/toolchain-gccarmnoneeabi/bin", + "toolchainBinDir": "C:/Users/tai/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin", "internalConsoleOptions": "openOnSessionStart", "svdPath": "C:/Users/tai/.platformio/platforms/ststm32/misc/svd/STM32F40x.svd", "preLaunchTask": { @@ -28,7 +28,7 @@ "name": "PIO Debug (skip Pre-Debug)", "executable": "e:/TAI/01_AKB/STM32CubeIDE/STM32F4xx/.pio/build/btt_skr_pro_1_1/firmware.elf", "projectEnvName": "btt_skr_pro_1_1", - "toolchainBinDir": "C:/Users/tai/.platformio/packages/toolchain-gccarmnoneeabi/bin", + "toolchainBinDir": "C:/Users/tai/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin", "internalConsoleOptions": "openOnSessionStart", "svdPath": "C:/Users/tai/.platformio/platforms/ststm32/misc/svd/STM32F40x.svd" }, @@ -38,7 +38,7 @@ "name": "PIO Debug (without uploading)", "executable": "e:/TAI/01_AKB/STM32CubeIDE/STM32F4xx/.pio/build/btt_skr_pro_1_1/firmware.elf", "projectEnvName": "btt_skr_pro_1_1", - "toolchainBinDir": "C:/Users/tai/.platformio/packages/toolchain-gccarmnoneeabi/bin", + "toolchainBinDir": "C:/Users/tai/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin", "internalConsoleOptions": "openOnSessionStart", "svdPath": "C:/Users/tai/.platformio/platforms/ststm32/misc/svd/STM32F40x.svd", "loadMode": "manual" diff --git a/Inc/my_machine.h b/Inc/my_machine.h index 34b00205..f55e6b89 100644 --- a/Inc/my_machine.h +++ b/Inc/my_machine.h @@ -52,12 +52,12 @@ //#define USB_SERIAL_CDC 1 // Serial communication via native USB. #endif //#define BLUETOOTH_ENABLE 1 // Set to 1 for HC-05 module. Requires and claims one auxillary input pin. -#define VFD_ENABLE 101 // Set to 1 or 2 for Huanyang VFD spindle. More here https://github.com/grblHAL/Plugins_spindle +//#define VFD_ENABLE 101 // Set to 1 or 2 for Huanyang VFD spindle. More here https://github.com/grblHAL/Plugins_spindle //#define SHIHLIN_ENABLE 1 //#define DUAL_SPINDLE 1 // Uncomment for switching between VFD spindle and PWM output with $32 -#define MODBUS_ENABLE 3 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin, 3 for shihlin vfd (author:tai) +//#define MODBUS_ENABLE 3 // Set to 1 for auto direction, 2 for direction signal on auxillary output pin, 3 for shihlin vfd (author:tai) //#define SDCARD_ENABLE 2 // Run gcode programs from SD card. -//#define KEYPAD_ENABLE 1 // Set to 1 for I2C keypad, 2 for other input such as serial data +//#define KEYPAD_ENABLE 2 // Set to 1 for I2C keypad, 2 for other input such as serial data //#define ODOMETER_ENABLE 1 // Odometer plugin. //#define PPI_ENABLE 1 // Laser PPI plugin. To be completed. //#define LASER_COOLANT_ENABLE 1 // Laser coolant plugin. To be completed. diff --git a/Src/f4mpv4_hosoi.h b/Src/f4mpv4_hosoi.h index 77d0bc48..ac4f2555 100644 --- a/Src/f4mpv4_hosoi.h +++ b/Src/f4mpv4_hosoi.h @@ -8,6 +8,20 @@ #ifndef F4MPV4_HOSOI_H_ #define F4MPV4_HOSOI_H_ +#include +#include "main.h" +#include "driver.h" +#include "serial.h" + +#include "grbl/hal.h" +#include "grbl/limits.h" +#include "grbl/protocol.h" +#include "grbl/motor_pins.h" +#include "grbl/pin_bits_masks.h" +#include "grbl/stepdir_map.h" +#include "grbl/motion_control.h" +#include "grbl/state_machine.h" + #define PU1_Pin GPIO_PIN_2 #define PU1_GPIO_Port GPIOA #define DR1_Pin GPIO_PIN_3 @@ -97,4 +111,7 @@ #define X10tog HAL_GPIO_TogglePin(X10); #define X11tog HAL_GPIO_TogglePin(X11); + +static void encoder_handler(sys_state_t state); + #endif /* F4MPV4_HOSOI_H_ */ diff --git a/Src/f4mpv5_hosoi.c b/Src/f4mpv5_hosoi.c index f3067598..ae539168 100644 --- a/Src/f4mpv5_hosoi.c +++ b/Src/f4mpv5_hosoi.c @@ -5,58 +5,55 @@ * Author: tai */ -#include - #include "f4mpv4_hosoi.h" -#include "main.h" -#include "driver.h" -#include "serial.h" - -#include "grbl/hal.h" -#include "grbl/limits.h" -#include "grbl/protocol.h" -#include "grbl/motor_pins.h" -#include "grbl/pin_bits_masks.h" -#include "grbl/stepdir_map.h" - -TIM_HandleTypeDef htim2; +TIM_HandleTypeDef htim2, htim3, htim5; UART_HandleTypeDef huart4; static on_report_options_ptr on_report_options; static on_execute_realtime_ptr on_execute_realtime; stepper_t stepper = - { - 0 - }; + { + 0 + }; static bool encoder_interrupt_busy = false; -static void MX_TIM2_Init(void); +static coord_data_t target = + { + 0 + }, origin = + { + 0 + }; +// Initialize planner data struct for motion blocks. +static plan_line_data_t plan_data; + +static void encoder_init(void); static void encoder_handler(sys_state_t state); // Add info about our plugin to the $I report. static void on_report_my_options(bool newopt) - { - on_report_options(newopt); +{ +on_report_options(newopt); - if (!newopt) - hal.stream.write("[PLUGIN:Blink LED v1.00]" ASCII_EOL); - } +if (!newopt) + hal.stream.write("[PLUGIN:Blink LED v1.00]" ASCII_EOL); +} static void blink_led(sys_state_t state) - { +{ // static bool led_on = false; - static uint32_t ms = 0; +static uint32_t ms = 0; - if (hal.get_elapsed_ticks() >= ms) - { - ms = hal.get_elapsed_ticks() + 1000; //ms +if (hal.get_elapsed_ticks() >= ms) + { + ms = hal.get_elapsed_ticks() + 1000; //ms // HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_6); // Green // HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_7); // Blue - HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_8); // Red + HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_8); // Red // Alternative Method // static bool led_on = false; @@ -72,95 +69,351 @@ static void blink_led(sys_state_t state) // GPIOC->ODR &= ~GPIO_PIN_13; // } - } + // temp - to be deleted +// target.x = sys.position[0] + 20.0f; +// target.y = sys.position[1]; +// hal.stream.write(uitoa(x)); +// hal.stream.write(","); +// hal.stream.write(uitoa(y)); +// hal.stream.write(ASCII_EOL); +// mc_line(target.values, &plan_data); - on_execute_realtime(state); } -//#define SLOW_JOG -#define FAST_JOG +on_execute_realtime(state); +} + +// ENCODER +#define NUMBER_OF_ENCODER 3 volatile int step = 0; volatile uint32_t counter = 0; volatile int16_t count = 0; volatile int16_t prev_count = 0; volatile int16_t delta_count = 0; - -void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) +static volatile float sign; + +typedef struct +{ + volatile uint32_t counter; + volatile int16_t count; + volatile int16_t prev_count; + volatile int16_t delta_count; + TIM_HandleTypeDef *timer; +} knob_t; + +static knob_t knob_x, knob_y, knob_z; +static knob_t *knob_ptr[NUMBER_OF_ENCODER] = + { + &knob_x, &knob_y, &knob_z + }; + +void TIM2_IRQHandler(void) +{ +HAL_TIM_IRQHandler(&htim2); +} +void TIM3_IRQHandler(void) +{ +HAL_TIM_IRQHandler(&htim3); +} +//void TIM5_IRQHandler(void) +//{ +//HAL_TIM_IRQHandler(&htim5); +//} + +void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim_encoder) +{ +GPIO_InitTypeDef GPIO_InitStruct = + { + 0 + }; +if (htim_encoder->Instance == TIM2) { - /* Prevent unused argument(s) compilation warning */ - UNUSED(htim); + __HAL_RCC_TIM2_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + GPIO_InitStruct.Pin = GPIO_PIN_15; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF1_TIM2; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + GPIO_InitStruct.Pin = GPIO_PIN_3; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF1_TIM2; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + HAL_NVIC_SetPriority(TIM2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(TIM2_IRQn); + } +else if (htim_encoder->Instance == TIM3) + { + __HAL_RCC_TIM3_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + GPIO_InitStruct.Pin = GPIO_PIN_4 | GPIO_PIN_5; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF2_TIM3; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + HAL_NVIC_SetPriority(TIM3_IRQn, 0, 1); + HAL_NVIC_EnableIRQ(TIM3_IRQn); + } +//else if (htim_encoder->Instance == TIM5) +// { +// /* USER CODE BEGIN TIM5_MspInit 0 */ +// +// /* USER CODE END TIM5_MspInit 0 */ +// /* Peripheral clock enable */ +// __HAL_RCC_TIM5_CLK_ENABLE(); +// +// __HAL_RCC_GPIOA_CLK_ENABLE(); +// /**TIM5 GPIO Configuration +// PA0-WKUP ------> TIM5_CH1 +// PA1 ------> TIM5_CH2 +// */ +// GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1; +// GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; +// GPIO_InitStruct.Pull = GPIO_NOPULL; +// GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; +// GPIO_InitStruct.Alternate = GPIO_AF2_TIM5; +// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); +// +// /* TIM5 interrupt Init */ +// HAL_NVIC_SetPriority(TIM5_IRQn, 0, 0); +// HAL_NVIC_EnableIRQ(TIM5_IRQn); +// /* USER CODE BEGIN TIM5_MspInit 1 */ +// +// /* USER CODE END TIM5_MspInit 1 */ +// } -// HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_6); - counter = __HAL_TIM_GET_COUNTER(htim); - count = (int16_t) (counter / 4); - delta_count = count - prev_count; +} + +void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef *htim_encoder) +{ +if (htim_encoder->Instance == TIM2) + { + __HAL_RCC_TIM2_CLK_DISABLE(); + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_15); + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_3); + HAL_NVIC_DisableIRQ(TIM2_IRQn); + } +else if (htim_encoder->Instance == TIM3) + { + __HAL_RCC_TIM3_CLK_DISABLE(); + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_4 | GPIO_PIN_5); + HAL_NVIC_DisableIRQ(TIM3_IRQn); } +//else if (htim_encoder->Instance == TIM5) +// { +// __HAL_RCC_TIM5_CLK_DISABLE(); +// HAL_GPIO_DeInit(GPIOA, GPIO_PIN_0 | GPIO_PIN_1); +// HAL_NVIC_DisableIRQ(TIM5_IRQn); +// } -static void MX_TIM2_Init(void) +} + +int temp = 0; + +void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) +{ +/* Prevent unused argument(s) compilation warning */ +UNUSED(htim); +//// HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_6); +// counter = __HAL_TIM_GET_COUNTER(htim); +// count = (int16_t) (counter / 4); +// delta_count = count - prev_count; +// if ((delta_count < 0) || (delta_count > 0)) +// { +// protocol_enqueue_rt_command(encoder_handler); +// } + +if (htim == knob_x.timer) { - TIM_Encoder_InitTypeDef sConfig = - { - 0 - }; - TIM_MasterConfigTypeDef sMasterConfig = - { - 0 - }; - htim2.Instance = TIM2; - htim2.Init.Prescaler = 0; - htim2.Init.CounterMode = TIM_COUNTERMODE_UP; - htim2.Init.Period = 4294967295; - htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - sConfig.EncoderMode = TIM_ENCODERMODE_TI12; - sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; - sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; - sConfig.IC1Prescaler = TIM_ICPSC_DIV1; - sConfig.IC1Filter = 0; - sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; - sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; - sConfig.IC2Prescaler = TIM_ICPSC_DIV1; - sConfig.IC2Filter = 0; - if (HAL_TIM_Encoder_Init(&htim2, &sConfig) != HAL_OK) + knob_x.counter = __HAL_TIM_GET_COUNTER(htim); + knob_x.count = (int16_t) (knob_x.counter / 4); + knob_x.delta_count = knob_x.count - knob_x.prev_count; + if (knob_x.delta_count != 0) { - Error_Handler(); + protocol_enqueue_rt_command(encoder_handler); } - sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) + } + +//if (htim == knob_z.timer) +// { +// temp = __HAL_TIM_GET_COUNTER(htim); +// knob_z.counter = __HAL_TIM_GET_COUNTER(htim); +// knob_z.count = (int16_t) (knob_z.counter / 4); +// knob_z.delta_count = knob_z.count - knob_z.prev_count; +// if (knob_z.delta_count != 0) +// { +// protocol_enqueue_rt_command(encoder_handler); +// } +// } + +temp = __HAL_TIM_GET_COUNTER(htim); + +} + +static void encoder_init(void) +{ +// Encoder 1 - OKAY!!! Do not edit +TIM_Encoder_InitTypeDef sConfig = { - Error_Handler(); - } + 0 + }; +TIM_MasterConfigTypeDef sMasterConfig = + { + 0 + }; + +//---------------------------------------- +htim2.Instance = TIM2; +htim2.Init.Prescaler = 0; +htim2.Init.CounterMode = TIM_COUNTERMODE_UP; +htim2.Init.Period = 4294967295; +htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; +htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; +sConfig.EncoderMode = TIM_ENCODERMODE_TI12; +sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; +sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; +sConfig.IC1Prescaler = TIM_ICPSC_DIV1; +sConfig.IC1Filter = 0; +sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; +sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; +sConfig.IC2Prescaler = TIM_ICPSC_DIV1; +sConfig.IC2Filter = 0; +if (HAL_TIM_Encoder_Init(&htim2, &sConfig) != HAL_OK) + { + Error_Handler(); + } +sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; +sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; +if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) + { + Error_Handler(); + } +//---------------------------------------- +//htim5.Instance = TIM5; +//htim5.Init.Prescaler = 0; +//htim5.Init.CounterMode = TIM_COUNTERMODE_UP; +//htim5.Init.Period = 4294967295; +//htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; +//htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; +//sConfig.EncoderMode = TIM_ENCODERMODE_TI12; +//sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; +//sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; +//sConfig.IC1Prescaler = TIM_ICPSC_DIV1; +//sConfig.IC1Filter = 0; +//sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; +//sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; +//sConfig.IC2Prescaler = TIM_ICPSC_DIV1; +//sConfig.IC2Filter = 0; +//if (HAL_TIM_Encoder_Init(&htim5, &sConfig) != HAL_OK) +// { +// Error_Handler(); +// } +//sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; +//sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; +//if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK) +// { +// Error_Handler(); +// } +//---------------------------------------- +htim3.Instance = TIM3; +htim3.Init.Prescaler = 0; +htim3.Init.CounterMode = TIM_COUNTERMODE_UP; +htim3.Init.Period = 65535; +htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; +htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; +sConfig.EncoderMode = TIM_ENCODERMODE_TI12; +sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; +sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; +sConfig.IC1Prescaler = TIM_ICPSC_DIV1; +sConfig.IC1Filter = 0; +sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; +sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; +sConfig.IC2Prescaler = TIM_ICPSC_DIV1; +sConfig.IC2Filter = 0; +if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK) + { + Error_Handler(); + } +sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; +sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; +if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) + { + Error_Handler(); } -void led_init(void) +//---------------------------------------- +knob_x.timer = &htim2; +//knob_y.timer = &htim5; +knob_z.timer = &htim3; +for (uint8_t i = 0; i < NUMBER_OF_ENCODER; i++) { - GPIO_InitTypeDef GPIO_InitStruct; + knob_ptr[i]->count = 0; + knob_ptr[i]->counter = 0; + knob_ptr[i]->delta_count = 0; + knob_ptr[i]->prev_count = 0; + } +HAL_TIM_Encoder_Start_IT(knob_x.timer, TIM_CHANNEL_ALL); +//HAL_TIM_Encoder_Start_IT(knob_y.timer, TIM_CHANNEL_ALL); +HAL_TIM_Encoder_Start_IT(knob_z.timer, TIM_CHANNEL_ALL); +} - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; +static void encoder_handler(sys_state_t state) +{ +if (encoder_interrupt_busy == true) return; +if (knob_x.delta_count != 0) + { + encoder_interrupt_busy = true; + sign = knob_x.delta_count >= 0 ? 1.0f : -1.0f; + plan_line_data_t plan_data = + { + 0 + }; + plan_data.feed_rate = 30000.0f; + protocol_buffer_synchronize(); + sync_position(); + // Get current position. + system_convert_array_steps_to_mpos(origin.values, sys.position); + target.x = origin.x + sign * 10.0f; + knob_x.prev_count = knob_x.count; + encoder_interrupt_busy = false; + mc_line(target.values, &plan_data); + } - GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8; - HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); +} - HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8, - GPIO_PIN_RESET); - } +void led_init(void) +{ +GPIO_InitTypeDef GPIO_InitStruct; + +GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; +GPIO_InitStruct.Pull = GPIO_NOPULL; +GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + +GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8; +HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + +HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8, + GPIO_PIN_RESET); +} void output_init(void) - { +{ - GPIO_InitTypeDef GPIO_InitStruct; +GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; +GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; +GPIO_InitStruct.Pull = GPIO_NOPULL; +GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - //X0-X11 - GPIO_InitStruct.Pin = GPIO_PIN_All; - HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); +//X0-X11 +GPIO_InitStruct.Pin = GPIO_PIN_All; +HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); ////PU1,DR1-PU8-DR8 // GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5 | GPIO_PIN_6 @@ -177,88 +430,7 @@ void output_init(void) ////KEY // GPIO_InitStruct.Pin = GPIO_PIN_12; // HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - } - -static void encoder_handler(sys_state_t state) - { - - if (encoder_interrupt_busy == true) - return; - - if ((delta_count < 0) || (delta_count > 0)) - { - encoder_interrupt_busy = true; - } - - if (delta_count > 0) - { - -#ifdef SLOW_JOG - - stepper.step_outbits.x = 1; - stepper.dir_outbits.x = 1; - stepper.dir_change = 1; // set when dir_outbits is changed. - hal.stepper.pulse_start(&stepper); - // hal.stepper. - sys.position[0] = sys.position[0] + (stepper.dir_outbits.x ? -1 : 1); - sync_position(); -#endif -#ifdef FAST_JOG - for (uint16_t step = 0; step < 100; step++) - { - stepper.step_outbits.x = 1; - stepper.dir_outbits.x = 1; - stepper.dir_change = 1; // set when dir_outbits is changed. - hal.stepper.pulse_start(&stepper); - - sys.position[0] = sys.position[0] + (stepper.dir_outbits.x ? -1 : 1); - sync_position(); - - hal.delay_ms(1, NULL); - - } - // stepper.step_outbits.x = 1; - // stepper.dir_outbits.x = 1; - // stepper.dir_change = 1; // set when dir_outbits is changed. - // hal.stepper.pulse_start(&stepper); - // sys.position[0] = sys.position[0] + (stepper.dir_outbits.x ? -1 : 1); - // sync_position(); - -#endif - - } - else if (delta_count < 0) - { - -#ifdef SLOW_JOG - stepper.step_outbits.x = 1; - stepper.dir_outbits.x = 0; - stepper.dir_change = 1; // set when dir_outbits is changed. - hal.stepper.pulse_start(&stepper); - sys.position[0] = sys.position[0] + (stepper.dir_outbits.x ? -1 : 1); - sync_position(); -#endif -#ifdef FAST_JOG - for (uint16_t step = 0; step < 100; step++) - { - stepper.step_outbits.x = 1; - stepper.dir_outbits.x = 0; - stepper.dir_change = 1; // set when dir_outbits is changed. - hal.stepper.pulse_start(&stepper); - - sys.position[0] = sys.position[0] + (stepper.dir_outbits.x ? -1 : 1); - sync_position(); - - hal.delay_ms(1, NULL); - - } - -#endif - - } - prev_count = count; - encoder_interrupt_busy = false; - } +} /************************************ * @@ -267,15 +439,14 @@ static void encoder_handler(sys_state_t state) ************************************/ const io_stream_t* serial3Init(uint32_t baud_rate); const io_stream_t* serial4Init(uint32_t baud_rate); - static stream_rx_buffer_t rxbuf3 = - { - 0 - }; + { + 0 + }; static stream_tx_buffer_t txbuf3 = - { - 0 - }; + { + 0 + }; static enqueue_realtime_command_ptr enqueue_realtime_command3 = protocol_enqueue_realtime_command; @@ -283,280 +454,282 @@ static enqueue_realtime_command_ptr enqueue_realtime_command3 = #define USART_GRBL_IRQHandler USART3_IRQHandler static io_stream_properties_t serial[] = - { - { - .type = StreamType_Serial, - .instance = 2, - .flags.claimable = On, - .flags.claimed = Off, - .flags.connected = On, - .flags.can_set_baud = - On, - .claim = serial3Init - }, - - { - .type = StreamType_Serial, - .instance = 3, - .flags.claimable = On, - .flags.claimed = Off, - .flags.connected = On, - .flags.can_set_baud = - On, - .flags.modbus_ready = On, - .claim = serial4Init - } + { + { + .type = StreamType_Serial, + .instance = 2, + .flags.claimable = On, + .flags.claimed = Off, + .flags.connected = On, + .flags.can_set_baud = + On, + .claim = serial3Init + }, - }; + { + .type = StreamType_Serial, + .instance = 3, + .flags.claimable = On, + .flags.claimed = Off, + .flags.connected = On, + .flags.can_set_baud = + On, + .flags.modbus_ready = On, + .claim = serial4Init + } + + }; // // Returns number of free characters in serial input buffer // static uint16_t serial3RxFree(void) - { - uint16_t tail = rxbuf3.tail, head = rxbuf3.head; +{ +uint16_t tail = rxbuf3.tail, head = rxbuf3.head; - return RX_BUFFER_SIZE - BUFCOUNT(head, tail, RX_BUFFER_SIZE); - } +return RX_BUFFER_SIZE - BUFCOUNT(head, tail, RX_BUFFER_SIZE); +} + +//endregion // // Flushes the serial input buffer // static void serial3RxFlush(void) - { - rxbuf3.tail = rxbuf3.head; - } +{ +rxbuf3.tail = rxbuf3.head; +} // // Flushes and adds a CAN character to the serial input buffer // static void serial3RxCancel(void) - { - rxbuf3.data[rxbuf3.head] = ASCII_CAN; - rxbuf3.tail = rxbuf3.head; - rxbuf3.head = BUFNEXT(rxbuf3.head, rxbuf3); - } +{ +rxbuf3.data[rxbuf3.head] = ASCII_CAN; +rxbuf3.tail = rxbuf3.head; +rxbuf3.head = BUFNEXT(rxbuf3.head, rxbuf3); +} // // Writes a character to the serial output stream // static bool serial3PutC(const char c) - { - uint16_t next_head = BUFNEXT(txbuf3.head, txbuf3); // Get pointer to next free slot in buffer - - while (txbuf3.tail == next_head) - { // While TX buffer full - if (!hal.stream_blocking_callback()) // check if blocking for space, - return false; // exit if not (leaves TX buffer in an inconsistent state) - } - txbuf3.data[txbuf3.head] = c; // Add data to buffer, - txbuf3.head = next_head; // update head pointer and - USART_GRBL->CR1 |= USART_CR1_TXEIE; // enable TX interrupts +{ +uint16_t next_head = BUFNEXT(txbuf3.head, txbuf3); // Get pointer to next free slot in buffer - return true; +while (txbuf3.tail == next_head) + { // While TX buffer full + if (!hal.stream_blocking_callback()) // check if blocking for space, + return false; // exit if not (leaves TX buffer in an inconsistent state) } +txbuf3.data[txbuf3.head] = c; // Add data to buffer, +txbuf3.head = next_head; // update head pointer and +USART_GRBL->CR1 |= USART_CR1_TXEIE; // enable TX interrupts + +return true; +} // // Writes a null terminated string to the serial output stream, blocks if buffer full // static void serial3WriteS(const char *s) - { - char c, *ptr = (char*) s; +{ +char c, *ptr = (char*) s; - while ((c = *ptr++) != '\0') - serial3PutC(c); - } +while ((c = *ptr++) != '\0') + serial3PutC(c); +} // // Writes a number of characters from string to the serial output stream followed by EOL, blocks if buffer full // static void serial3Write(const char *s, uint16_t length) - { - char *ptr = (char*) s; +{ +char *ptr = (char*) s; - while (length--) - serial3PutC(*ptr++); - } +while (length--) + serial3PutC(*ptr++); +} // // serialGetC - returns -1 if no data available // static int16_t serial3GetC(void) - { - uint_fast16_t tail = rxbuf3.tail; // Get buffer pointer +{ +uint_fast16_t tail = rxbuf3.tail; // Get buffer pointer - if (tail == rxbuf3.head) - return -1; // no data available +if (tail == rxbuf3.head) + return -1; // no data available - char data = rxbuf3.data[tail]; // Get next character - rxbuf3.tail = BUFNEXT(tail, rxbuf3); // and update pointer +char data = rxbuf3.data[tail]; // Get next character +rxbuf3.tail = BUFNEXT(tail, rxbuf3); // and update pointer - return (int16_t) data; - } +return (int16_t) data; +} static bool serial3SuspendInput(bool suspend) - { - return stream_rx_suspend(&rxbuf3, suspend); - } +{ +return stream_rx_suspend(&rxbuf3, suspend); +} static bool serial3SetBaudRate(uint32_t baud_rate) - { +{ // if USART 1/6/9/10 → HAL_RCC_GetPCLK2Freq() - else: HAL_RCC_GetPCLK1Freq() - USART_GRBL->CR1 = USART_CR1_RE | USART_CR1_TE; - USART_GRBL->BRR = UART_BRR_SAMPLING16(HAL_RCC_GetPCLK1Freq(), baud_rate); - USART_GRBL->CR1 |= (USART_CR1_UE | USART_CR1_RXNEIE); +USART_GRBL->CR1 = USART_CR1_RE | USART_CR1_TE; +USART_GRBL->BRR = UART_BRR_SAMPLING16(HAL_RCC_GetPCLK1Freq(), baud_rate); +USART_GRBL->CR1 |= (USART_CR1_UE | USART_CR1_RXNEIE); - return true; - } +return true; +} static bool serial3Disable(bool disable) - { - if (disable) - USART_GRBL->CR1 &= ~USART_CR1_RXNEIE; - else - USART_GRBL->CR1 |= USART_CR1_RXNEIE; +{ +if (disable) + USART_GRBL->CR1 &= ~USART_CR1_RXNEIE; +else + USART_GRBL->CR1 |= USART_CR1_RXNEIE; - return true; - } +return true; +} static bool serial3EnqueueRtCommand(char c) - { - return enqueue_realtime_command3(c); - } +{ +return enqueue_realtime_command3(c); +} static enqueue_realtime_command_ptr serial3SetRtHandler( enqueue_realtime_command_ptr handler) - { - enqueue_realtime_command_ptr prev = enqueue_realtime_command3; +{ +enqueue_realtime_command_ptr prev = enqueue_realtime_command3; - if (handler) - enqueue_realtime_command3 = handler; +if (handler) + enqueue_realtime_command3 = handler; - return prev; - } +return prev; +} const io_stream_t* serial3Init(uint32_t baud_rate) - { +{ - static const io_stream_t stream = - { - .type = StreamType_Serial, - .state.connected = On, - .read = serial3GetC, - .write = serial3WriteS, - .write_n = - serial3Write, - .write_char = serial3PutC, - .enqueue_rt_command = serial3EnqueueRtCommand, - .get_rx_buffer_free = - serial3RxFree, - // .get_rx_buffer_count = serial3RxCount, +static const io_stream_t stream = + { + .type = StreamType_Serial, + .state.connected = On, + .read = serial3GetC, + .write = serial3WriteS, + .write_n = + serial3Write, + .write_char = serial3PutC, + .enqueue_rt_command = serial3EnqueueRtCommand, + .get_rx_buffer_free = + serial3RxFree, + // .get_rx_buffer_count = serial3RxCount, // .get_tx_buffer_count = serial3TxCount, // .reset_write_buffer = serial3TxFlush, - .reset_read_buffer = serial3RxFlush, - .cancel_read_buffer = serial3RxCancel, - .suspend_read = serial3SuspendInput, - .disable_rx = serial3Disable, - .set_baud_rate = serial3SetBaudRate, - .set_enqueue_rt_handler = serial3SetRtHandler - }; - - if (serial[0].flags.claimed) - return NULL; - - serial[0].flags.claimed = On; - - GPIO_InitTypeDef GPIO_InitStructure = - { - 0 - }; - - GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; - GPIO_InitStructure.Pull = GPIO_NOPULL; - GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - - __HAL_RCC_GPIOB_CLK_ENABLE(); // USART3 Port - __HAL_RCC_USART3_CLK_ENABLE(); - - GPIO_InitStructure.Pin = GPIO_PIN_10 | GPIO_PIN_11; - GPIO_InitStructure.Alternate = GPIO_AF7_USART3; - HAL_GPIO_Init(GPIOB, &GPIO_InitStructure); - - serial3SetBaudRate(baud_rate); - - HAL_NVIC_SetPriority(USART3_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(USART3_IRQn); - - static const periph_pin_t tx = - { - .function = Output_TX, - .group = PinGroup_UART3, - .port = GPIOB, - .pin = 10, - .mode = - { - .mask = PINMODE_OUTPUT - }, - .description = "UART3" - }; - - static const periph_pin_t rx = - { - .function = Input_RX, - .group = PinGroup_UART3, - .port = GPIOB, - .pin = 11, - .mode = - { - .mask = PINMODE_NONE - }, - .description = "UART3" - }; - - hal.periph_port.register_pin(&rx); - hal.periph_port.register_pin(&tx); - - return &stream; - } + .reset_read_buffer = serial3RxFlush, + .cancel_read_buffer = serial3RxCancel, + .suspend_read = serial3SuspendInput, + .disable_rx = serial3Disable, + .set_baud_rate = serial3SetBaudRate, + .set_enqueue_rt_handler = serial3SetRtHandler + }; + +if (serial[0].flags.claimed) + return NULL; + +serial[0].flags.claimed = On; + +GPIO_InitTypeDef GPIO_InitStructure = + { + 0 + }; + +GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; +GPIO_InitStructure.Pull = GPIO_NOPULL; +GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + +__HAL_RCC_GPIOB_CLK_ENABLE(); // USART3 Port +__HAL_RCC_USART3_CLK_ENABLE(); + +GPIO_InitStructure.Pin = GPIO_PIN_10 | GPIO_PIN_11; +GPIO_InitStructure.Alternate = GPIO_AF7_USART3; +HAL_GPIO_Init(GPIOB, &GPIO_InitStructure); + +serial3SetBaudRate(baud_rate); + +HAL_NVIC_SetPriority(USART3_IRQn, 0, 0); +HAL_NVIC_EnableIRQ(USART3_IRQn); + +static const periph_pin_t tx = + { + .function = Output_TX, + .group = PinGroup_UART3, + .port = GPIOB, + .pin = 10, + .mode = + { + .mask = PINMODE_OUTPUT + }, + .description = "UART3" + }; + +static const periph_pin_t rx = + { + .function = Input_RX, + .group = PinGroup_UART3, + .port = GPIOB, + .pin = 11, + .mode = + { + .mask = PINMODE_NONE + }, + .description = "UART3" + }; + +hal.periph_port.register_pin(&rx); +hal.periph_port.register_pin(&tx); + +return &stream; +} void USART_GRBL_IRQHandler(void) +{ +if (USART_GRBL->SR & USART_SR_RXNE) { - if (USART_GRBL->SR & USART_SR_RXNE) - { - uint32_t data = USART_GRBL->DR; - if (!enqueue_realtime_command3((char) data)) - { // Check and strip realtime commands... - uint16_t next_head = BUFNEXT(rxbuf3.head, rxbuf3); // Get and increment buffer pointer - if (next_head == rxbuf3.tail) // If buffer full - rxbuf3.overflow = 1; // flag overflow - else - { - rxbuf3.data[rxbuf3.head] = (char) data; // if not add data to buffer - rxbuf3.head = next_head; // and update pointer - } + uint32_t data = USART_GRBL->DR; + if (!enqueue_realtime_command3((char) data)) + { // Check and strip realtime commands... + uint16_t next_head = BUFNEXT(rxbuf3.head, rxbuf3); // Get and increment buffer pointer + if (next_head == rxbuf3.tail) // If buffer full + rxbuf3.overflow = 1; // flag overflow + else + { + rxbuf3.data[rxbuf3.head] = (char) data; // if not add data to buffer + rxbuf3.head = next_head; // and update pointer } } + } - if ((USART_GRBL->SR & USART_SR_TXE) && (USART_GRBL->CR1 & USART_CR1_TXEIE)) - { - uint_fast16_t tail = txbuf3.tail; // Get buffer pointer - USART_GRBL->DR = txbuf3.data[tail]; // Send next character - txbuf3.tail = tail = BUFNEXT(tail, txbuf3); // and increment pointer - if (tail == txbuf3.head) // If buffer empty then - USART_GRBL->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt - } +if ((USART_GRBL->SR & USART_SR_TXE) && (USART_GRBL->CR1 & USART_CR1_TXEIE)) + { + uint_fast16_t tail = txbuf3.tail; // Get buffer pointer + USART_GRBL->DR = txbuf3.data[tail]; // Send next character + txbuf3.tail = tail = BUFNEXT(tail, txbuf3); // and increment pointer + if (tail == txbuf3.head) // If buffer empty then + USART_GRBL->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt } +} // SERIAL 4 static stream_rx_buffer_t rxbuf4 = - { - 0 - }; + { + 0 + }; static stream_tx_buffer_t txbuf4 = - { - 0 - }; + { + 0 + }; static enqueue_realtime_command_ptr enqueue_realtime_command4 = protocol_enqueue_realtime_command; @@ -567,283 +740,289 @@ static enqueue_realtime_command_ptr enqueue_realtime_command4 = // Returns number of free characters in serial input buffer // static uint16_t serial4RxFree(void) - { - uint16_t tail = rxbuf4.tail, head = rxbuf4.head; +{ +uint16_t tail = rxbuf4.tail, head = rxbuf4.head; - return RX_BUFFER_SIZE - BUFCOUNT(head, tail, RX_BUFFER_SIZE); - } +return RX_BUFFER_SIZE - BUFCOUNT(head, tail, RX_BUFFER_SIZE); +} // // Returns number of characters in serial input buffer // static uint16_t serial4RxCount(void) - { - uint32_t tail = rxbuf4.tail, head = rxbuf4.head; +{ +uint32_t tail = rxbuf4.tail, head = rxbuf4.head; - return BUFCOUNT(head, tail, RX_BUFFER_SIZE); - } +return BUFCOUNT(head, tail, RX_BUFFER_SIZE); +} // // Flushes the serial input buffer // static void serial4RxFlush(void) - { - rxbuf4.tail = rxbuf4.head; - } +{ +rxbuf4.tail = rxbuf4.head; +} // // Flushes and adds a CAN character to the serial input buffer // static void serial4RxCancel(void) - { - rxbuf4.data[rxbuf4.head] = ASCII_CAN; - rxbuf4.tail = rxbuf4.head; - rxbuf4.head = BUFNEXT(rxbuf4.head, rxbuf4); - } +{ +rxbuf4.data[rxbuf4.head] = ASCII_CAN; +rxbuf4.tail = rxbuf4.head; +rxbuf4.head = BUFNEXT(rxbuf4.head, rxbuf4); +} // // Writes a character to the serial output stream // static bool serial4PutC(const char c) - { - uint16_t next_head = BUFNEXT(txbuf4.head, txbuf4); // Get pointer to next free slot in buffer +{ +uint16_t next_head = BUFNEXT(txbuf4.head, txbuf4); // Get pointer to next free slot in buffer - while (txbuf4.tail == next_head) - { // While TX buffer full - if (!hal.stream_blocking_callback()) // check if blocking for space, - return false; // exit if not (leaves TX buffer in an inconsistent state) - } - txbuf4.data[txbuf4.head] = c; // Add data to buffer, - txbuf4.head = next_head; // update head pointer and - USART_MODBUS->CR1 |= USART_CR1_TXEIE; // enable TX interrupts - - return true; +while (txbuf4.tail == next_head) + { // While TX buffer full + if (!hal.stream_blocking_callback()) // check if blocking for space, + return false; // exit if not (leaves TX buffer in an inconsistent state) } +txbuf4.data[txbuf4.head] = c; // Add data to buffer, +txbuf4.head = next_head; // update head pointer and +USART_MODBUS->CR1 |= USART_CR1_TXEIE; // enable TX interrupts + +return true; +} // // Writes a null terminated string to the serial output stream, blocks if buffer full // static void serial4WriteS(const char *s) - { - char c, *ptr = (char*) s; +{ +char c, *ptr = (char*) s; - while ((c = *ptr++) != '\0') - serial4PutC(c); - } +while ((c = *ptr++) != '\0') + serial4PutC(c); +} // // Writes a number of characters from string to the serial output stream followed by EOL, blocks if buffer full // static void serial4Write(const char *s, uint16_t length) - { - char *ptr = (char*) s; +{ +char *ptr = (char*) s; - while (length--) - serial4PutC(*ptr++); - } +while (length--) + serial4PutC(*ptr++); +} // // Flushes the serial output buffer // static void serial4TxFlush(void) - { - UART4->CR1 &= ~USART_CR1_TXEIE; // Disable TX interrupts - txbuf4.tail = txbuf4.head; - } +{ +UART4->CR1 &= ~USART_CR1_TXEIE; // Disable TX interrupts +txbuf4.tail = txbuf4.head; +} // // Returns number of characters pending transmission // static uint16_t serial4TxCount(void) - { - uint32_t tail = txbuf4.tail, head = txbuf4.head; +{ +uint32_t tail = txbuf4.tail, head = txbuf4.head; - return BUFCOUNT(head, tail, TX_BUFFER_SIZE) - + (UART4->SR & USART_SR_TC ? 0 : 1); - } +return BUFCOUNT(head, tail, TX_BUFFER_SIZE) + + (UART4->SR & USART_SR_TC ? 0 : 1); +} // // serialGetC - returns -1 if no data available // static int16_t serial4GetC(void) - { - uint_fast16_t tail = rxbuf4.tail; // Get buffer pointer +{ +uint_fast16_t tail = rxbuf4.tail; // Get buffer pointer - if (tail == rxbuf4.head) - return -1; // no data available +if (tail == rxbuf4.head) + return -1; // no data available - char data = rxbuf4.data[tail]; // Get next character - rxbuf4.tail = BUFNEXT(tail, rxbuf4); // and update pointer +char data = rxbuf4.data[tail]; // Get next character +rxbuf4.tail = BUFNEXT(tail, rxbuf4); // and update pointer - return (int16_t) data; - } +return (int16_t) data; +} static bool serial4SuspendInput(bool suspend) - { - return stream_rx_suspend(&rxbuf4, suspend); - } +{ +return stream_rx_suspend(&rxbuf4, suspend); +} static bool serial4SetBaudRate(uint32_t baud_rate) - { +{ // if USART 1/6/9/10 → HAL_RCC_GetPCLK2Freq() - else: HAL_RCC_GetPCLK1Freq() - USART_MODBUS->CR1 = USART_CR1_RE | USART_CR1_TE; - USART_MODBUS->BRR = UART_BRR_SAMPLING16(HAL_RCC_GetPCLK1Freq(), 38400); - USART_MODBUS->CR1 |= (USART_CR1_UE | USART_CR1_RXNEIE); - - huart4.Instance = UART4; - huart4.Init.BaudRate = 38400; - huart4.Init.WordLength = UART_WORDLENGTH_9B; - huart4.Init.StopBits = UART_STOPBITS_1; - huart4.Init.Parity = UART_PARITY_EVEN; - huart4.Init.Mode = UART_MODE_TX_RX; - huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE; - huart4.Init.OverSampling = UART_OVERSAMPLING_16; - if (HAL_UART_Init(&huart4) != HAL_OK) - { - Error_Handler(); - } - - return true; +USART_MODBUS->CR1 = USART_CR1_RE | USART_CR1_TE; +USART_MODBUS->BRR = UART_BRR_SAMPLING16(HAL_RCC_GetPCLK1Freq(), 38400); +USART_MODBUS->CR1 |= (USART_CR1_UE | USART_CR1_RXNEIE); + +huart4.Instance = UART4; +huart4.Init.BaudRate = 38400; +huart4.Init.WordLength = UART_WORDLENGTH_9B; +huart4.Init.StopBits = UART_STOPBITS_1; +huart4.Init.Parity = UART_PARITY_EVEN; +huart4.Init.Mode = UART_MODE_TX_RX; +huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE; +huart4.Init.OverSampling = UART_OVERSAMPLING_16; +if (HAL_UART_Init(&huart4) != HAL_OK) + { + Error_Handler(); } +return true; +} + static bool serial4Disable(bool disable) - { - if (disable) - USART_MODBUS->CR1 &= ~USART_CR1_RXNEIE; - else - USART_MODBUS->CR1 |= USART_CR1_RXNEIE; +{ +if (disable) + USART_MODBUS->CR1 &= ~USART_CR1_RXNEIE; +else + USART_MODBUS->CR1 |= USART_CR1_RXNEIE; - return true; - } +return true; +} static bool serial4EnqueueRtCommand(char c) - { - return enqueue_realtime_command4(c); - } +{ +return enqueue_realtime_command4(c); +} static enqueue_realtime_command_ptr serial4SetRtHandler( enqueue_realtime_command_ptr handler) - { - enqueue_realtime_command_ptr prev = enqueue_realtime_command4; +{ +enqueue_realtime_command_ptr prev = enqueue_realtime_command4; - if (handler) - enqueue_realtime_command4 = handler; +if (handler) + enqueue_realtime_command4 = handler; - return prev; - } +return prev; +} const io_stream_t* serial4Init(uint32_t baud_rate) - { +{ - static const io_stream_t stream = - { - .type = StreamType_Serial, - .instance = 3, - .state.connected = On, - .read = serial4GetC, - .write = serial4WriteS, - .write_n = serial4Write, - .write_char = serial4PutC, - .enqueue_rt_command = serial4EnqueueRtCommand, - .get_rx_buffer_free = serial4RxFree, - .get_rx_buffer_count = serial4RxCount, - .get_tx_buffer_count = serial4TxCount, - .reset_write_buffer = serial4TxFlush, - .reset_read_buffer = serial4RxFlush, - .cancel_read_buffer = serial4RxCancel, - .suspend_read = serial4SuspendInput, - .disable_rx = serial4Disable, - .set_baud_rate = serial4SetBaudRate, - .set_enqueue_rt_handler = serial4SetRtHandler - }; - - if (serial[1].flags.claimed) - return NULL; - - serial[1].flags.claimed = On; - - GPIO_InitTypeDef GPIO_InitStructure = - { - 0 - }; - - GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; - GPIO_InitStructure.Pull = GPIO_NOPULL; - GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - - __HAL_RCC_GPIOC_CLK_ENABLE(); // USART4 Port - __HAL_RCC_UART4_CLK_ENABLE(); - - GPIO_InitStructure.Pin = GPIO_PIN_10 | GPIO_PIN_11; - GPIO_InitStructure.Alternate = GPIO_AF8_UART4; - HAL_GPIO_Init(GPIOC, &GPIO_InitStructure); - - serial4SetBaudRate(baud_rate); - - HAL_NVIC_SetPriority(UART4_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(UART4_IRQn); - - static const periph_pin_t tx = - { - .function = Output_TX, - .group = PinGroup_UART4, - .port = GPIOC, - .pin = 10, - .mode = - { - .mask = PINMODE_OUTPUT - }, - .description = "UART4" - }; - - static const periph_pin_t rx = - { - .function = Input_RX, - .group = PinGroup_UART4, - .port = GPIOC, - .pin = 11, - .mode = - { - .mask = PINMODE_NONE - }, - .description = "UART4" - }; - - hal.periph_port.register_pin(&rx); - hal.periph_port.register_pin(&tx); - - return &stream; - } +static const io_stream_t stream = + { + .type = StreamType_Serial, + .instance = 3, + .state.connected = On, + .read = serial4GetC, + .write = serial4WriteS, + .write_n = serial4Write, + .write_char = serial4PutC, + .enqueue_rt_command = serial4EnqueueRtCommand, + .get_rx_buffer_free = serial4RxFree, + .get_rx_buffer_count = serial4RxCount, + .get_tx_buffer_count = serial4TxCount, + .reset_write_buffer = serial4TxFlush, + .reset_read_buffer = serial4RxFlush, + .cancel_read_buffer = serial4RxCancel, + .suspend_read = serial4SuspendInput, + .disable_rx = serial4Disable, + .set_baud_rate = serial4SetBaudRate, + .set_enqueue_rt_handler = serial4SetRtHandler + }; + +if (serial[1].flags.claimed) + return NULL; + +serial[1].flags.claimed = On; + +GPIO_InitTypeDef GPIO_InitStructure = + { + 0 + }; + +GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; +GPIO_InitStructure.Pull = GPIO_NOPULL; +GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + +__HAL_RCC_GPIOC_CLK_ENABLE(); // USART4 Port +__HAL_RCC_UART4_CLK_ENABLE(); + +GPIO_InitStructure.Pin = GPIO_PIN_10 | GPIO_PIN_11; +GPIO_InitStructure.Alternate = GPIO_AF8_UART4; +HAL_GPIO_Init(GPIOC, &GPIO_InitStructure); + +serial4SetBaudRate(baud_rate); + +HAL_NVIC_SetPriority(UART4_IRQn, 0, 0); +HAL_NVIC_EnableIRQ(UART4_IRQn); + +static const periph_pin_t tx = + { + .function = Output_TX, + .group = PinGroup_UART4, + .port = GPIOC, + .pin = 10, + .mode = + { + .mask = PINMODE_OUTPUT + }, + .description = "UART4" + }; + +static const periph_pin_t rx = + { + .function = Input_RX, + .group = PinGroup_UART4, + .port = GPIOC, + .pin = 11, + .mode = + { + .mask = PINMODE_NONE + }, + .description = "UART4" + }; + +hal.periph_port.register_pin(&rx); +hal.periph_port.register_pin(&tx); + +return &stream; +} void USART_MODBUS_IRQHandler(void) +{ +if (USART_MODBUS->SR & USART_SR_RXNE) { - if (USART_MODBUS->SR & USART_SR_RXNE) - { - uint32_t data = USART_MODBUS->DR; - if (!enqueue_realtime_command4((char) data)) - { // Check and strip realtime commands... - uint16_t next_head = BUFNEXT(rxbuf4.head, rxbuf4); // Get and increment buffer pointer - if (next_head == rxbuf4.tail) // If buffer full - rxbuf4.overflow = 1; // flag overflow - else - { - rxbuf4.data[rxbuf4.head] = (char) data; // if not add data to buffer - rxbuf4.head = next_head; // and update pointer - } + uint32_t data = USART_MODBUS->DR; + if (!enqueue_realtime_command4((char) data)) + { // Check and strip realtime commands... + uint16_t next_head = BUFNEXT(rxbuf4.head, rxbuf4); // Get and increment buffer pointer + if (next_head == rxbuf4.tail) // If buffer full + rxbuf4.overflow = 1; // flag overflow + else + { + rxbuf4.data[rxbuf4.head] = (char) data; // if not add data to buffer + rxbuf4.head = next_head; // and update pointer } } + } - if ((USART_MODBUS->SR & USART_SR_TXE) - && (USART_MODBUS->CR1 & USART_CR1_TXEIE)) - { - uint_fast16_t tail = txbuf4.tail; // Get buffer pointer - USART_MODBUS->DR = txbuf4.data[tail]; // Send next character - txbuf4.tail = tail = BUFNEXT(tail, txbuf4); // and increment pointer - if (tail == txbuf4.head) // If buffer empty then - USART_MODBUS->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt - } +if ((USART_MODBUS->SR & USART_SR_TXE) + && (USART_MODBUS->CR1 & USART_CR1_TXEIE)) + { + uint_fast16_t tail = txbuf4.tail; // Get buffer pointer + USART_MODBUS->DR = txbuf4.data[tail]; // Send next character + txbuf4.tail = tail = BUFNEXT(tail, txbuf4); // and increment pointer + if (tail == txbuf4.head) // If buffer empty then + USART_MODBUS->CR1 &= ~USART_CR1_TXEIE; // disable UART TX interrupt } +} + +/************************************ + * + * MAIN CODE + * + ************************************/ /************************************ * @@ -851,34 +1030,37 @@ void USART_MODBUS_IRQHandler(void) * ************************************/ void board_init(void) - { +{ // Add info about our plugin to the $I report. - on_report_options = grbl.on_report_options; - grbl.on_report_options = on_report_my_options; +on_report_options = grbl.on_report_options; +grbl.on_report_options = on_report_my_options; // Add blink LED function to grblHAL foreground process - led_init(); - on_execute_realtime = grbl.on_execute_realtime; - grbl.on_execute_realtime = blink_led; +led_init(); +on_execute_realtime = grbl.on_execute_realtime; +grbl.on_execute_realtime = blink_led; // Enable F4MPV5 outputs - output_init(); +output_init(); // Enable Steppers - X0set - ; +X0set +; + +// Enable Encoder Timer +encoder_init(); +//HAL_TIM_Encoder_Start_IT(&htim2, TIM_CHANNEL_ALL); // Enable UART - static io_stream_details_t streams = - { - .n_streams = sizeof(serial) / sizeof(io_stream_properties_t), .streams = - serial, - }; - stream_register_streams(&streams); +static io_stream_details_t streams = + { + .n_streams = sizeof(serial) / sizeof(io_stream_properties_t), .streams = + serial, + }; +stream_register_streams(&streams); // Enable VFD Spindle Control -// Enable Encoder Timer -// MX_TIM2_Init(); -// HAL_TIM_Encoder_Start_IT(&htim2, TIM_CHANNEL_ALL); +// Enable Constant Jogging +memset(&plan_data, 0, sizeof(plan_line_data_t)); // Zero plan_data struct - } +} diff --git a/Src/stm32f4xx_hal_msp.c b/Src/stm32f4xx_hal_msp.c index 63f0d4ca..1df04fa7 100644 --- a/Src/stm32f4xx_hal_msp.c +++ b/Src/stm32f4xx_hal_msp.c @@ -1,22 +1,22 @@ /* USER CODE BEGIN Header */ /** - ****************************************************************************** - * File Name : stm32f4xx_hal_msp.c - * Description : This file provides code for the MSP Initialization - * and de-Initialization codes. - ****************************************************************************** - * @attention - * - *

© Copyright (c) 2020 STMicroelectronics. - * All rights reserved.

- * - * This software component is licensed by ST under Ultimate Liberty license - * SLA0044, the "License"; You may not use this file except in compliance with - * the License. You may obtain a copy of the License at: - * www.st.com/SLA0044 - * - ****************************************************************************** - */ + ****************************************************************************** + * File Name : stm32f4xx_hal_msp.c + * Description : This file provides code for the MSP Initialization + * and de-Initialization codes. + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2020 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under Ultimate Liberty license + * SLA0044, the "License"; You may not use this file except in compliance with + * the License. You may obtain a copy of the License at: + * www.st.com/SLA0044 + * + ****************************************************************************** + */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ @@ -32,7 +32,7 @@ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN Define */ - + /* USER CODE END Define */ /* Private macro -------------------------------------------------------------*/ @@ -59,28 +59,29 @@ /* USER CODE END 0 */ /** - * Initializes the Global MSP. - */ + * Initializes the Global MSP. + */ void HAL_MspInit(void) { - /* USER CODE BEGIN MspInit 0 */ +/* USER CODE BEGIN MspInit 0 */ - /* USER CODE END MspInit 0 */ +/* USER CODE END MspInit 0 */ - __HAL_RCC_SYSCFG_CLK_ENABLE(); - __HAL_RCC_PWR_CLK_ENABLE(); +__HAL_RCC_SYSCFG_CLK_ENABLE(); +__HAL_RCC_PWR_CLK_ENABLE(); - HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_0); +HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_0); - /* System interrupt init*/ +/* System interrupt init*/ - /* USER CODE BEGIN MspInit 1 */ +/* USER CODE BEGIN MspInit 1 */ - /* USER CODE END MspInit 1 */ +/* USER CODE END MspInit 1 */ } /* USER CODE BEGIN 1 */ + /* USER CODE END 1 */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ From caa11c8c8512fc7a16727b8a70054868dfa84598 Mon Sep 17 00:00:00 2001 From: atlesg Date: Fri, 21 Jan 2022 13:52:46 +0700 Subject: [PATCH 09/10] 2 encoder working - 2 encoder working using TIM2 and TIM3, TIM5 is conflicted with STEPPER TIMER - The 2 motors are not running synchronously --- Inc/my_machine_map.h | 4 +- Src/f4mpv5_hosoi.c | 167 +++++++++++++++++++++++++++---------------- 2 files changed, 108 insertions(+), 63 deletions(-) diff --git a/Inc/my_machine_map.h b/Inc/my_machine_map.h index a4871791..fee333cf 100644 --- a/Inc/my_machine_map.h +++ b/Inc/my_machine_map.h @@ -110,8 +110,8 @@ // Define flood and mist coolant enable output pins. #define COOLANT_FLOOD_PORT GPIOE #define COOLANT_FLOOD_PIN 1 -#define COOLANT_MIST_PORT GPIOB -#define COOLANT_MIST_PIN 4 +//#define COOLANT_MIST_PORT GPIOB +//#define COOLANT_MIST_PIN 4 // Define user-control controls (cycle start, reset, feed hold) input pins. #define CONTROL_PORT GPIOD diff --git a/Src/f4mpv5_hosoi.c b/Src/f4mpv5_hosoi.c index ae539168..ed4d8307 100644 --- a/Src/f4mpv5_hosoi.c +++ b/Src/f4mpv5_hosoi.c @@ -42,47 +42,6 @@ if (!newopt) hal.stream.write("[PLUGIN:Blink LED v1.00]" ASCII_EOL); } -static void blink_led(sys_state_t state) -{ -// static bool led_on = false; -static uint32_t ms = 0; - -if (hal.get_elapsed_ticks() >= ms) - { - ms = hal.get_elapsed_ticks() + 1000; //ms - -// HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_6); // Green -// HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_7); // Blue - HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_8); // Red - -// Alternative Method -// static bool led_on = false; -// static uint32_t ms = 0; -// -// if (hal.get_elapsed_ticks() >= ms) -// { -// ms = hal.get_elapsed_ticks() + 500; //ms -// led_on = !led_on; -// if (led_on) -// GPIOC->ODR |= GPIO_PIN_13; -// else -// GPIOC->ODR &= ~GPIO_PIN_13; -// } - - // temp - to be deleted -// target.x = sys.position[0] + 20.0f; -// target.y = sys.position[1]; -// hal.stream.write(uitoa(x)); -// hal.stream.write(","); -// hal.stream.write(uitoa(y)); -// hal.stream.write(ASCII_EOL); -// mc_line(target.values, &plan_data); - - } - -on_execute_realtime(state); -} - // ENCODER #define NUMBER_OF_ENCODER 3 @@ -99,6 +58,7 @@ typedef struct volatile int16_t count; volatile int16_t prev_count; volatile int16_t delta_count; + volatile float sign; TIM_HandleTypeDef *timer; } knob_t; @@ -144,7 +104,7 @@ if (htim_encoder->Instance == TIM2) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; GPIO_InitStruct.Alternate = GPIO_AF1_TIM2; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - HAL_NVIC_SetPriority(TIM2_IRQn, 0, 0); + HAL_NVIC_SetPriority(TIM2_IRQn, 2, 0); HAL_NVIC_EnableIRQ(TIM2_IRQn); } else if (htim_encoder->Instance == TIM3) @@ -157,7 +117,7 @@ else if (htim_encoder->Instance == TIM3) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; GPIO_InitStruct.Alternate = GPIO_AF2_TIM3; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - HAL_NVIC_SetPriority(TIM3_IRQn, 0, 1); + HAL_NVIC_SetPriority(TIM3_IRQn, 3, 0); HAL_NVIC_EnableIRQ(TIM3_IRQn); } //else if (htim_encoder->Instance == TIM5) @@ -233,26 +193,29 @@ if (htim == knob_x.timer) { knob_x.counter = __HAL_TIM_GET_COUNTER(htim); knob_x.count = (int16_t) (knob_x.counter / 4); - knob_x.delta_count = knob_x.count - knob_x.prev_count; - if (knob_x.delta_count != 0) - { - protocol_enqueue_rt_command(encoder_handler); - } + +// if (knob_x.delta_count != 0) +// { +// protocol_enqueue_rt_command(encoder_handler); +// } } -//if (htim == knob_z.timer) -// { -// temp = __HAL_TIM_GET_COUNTER(htim); -// knob_z.counter = __HAL_TIM_GET_COUNTER(htim); -// knob_z.count = (int16_t) (knob_z.counter / 4); -// knob_z.delta_count = knob_z.count - knob_z.prev_count; +if (htim == knob_z.timer) + { + temp = __HAL_TIM_GET_COUNTER(htim); + knob_z.counter = __HAL_TIM_GET_COUNTER(htim); + knob_z.count = (int16_t) (knob_z.counter / 4); + // if (knob_z.delta_count != 0) // { // protocol_enqueue_rt_command(encoder_handler); // } -// } + } +knob_x.delta_count = knob_x.count - knob_x.prev_count; +knob_z.delta_count = knob_z.count - knob_z.prev_count; + +protocol_enqueue_rt_command(encoder_handler); -temp = __HAL_TIM_GET_COUNTER(htim); } @@ -365,11 +328,52 @@ HAL_TIM_Encoder_Start_IT(knob_z.timer, TIM_CHANNEL_ALL); static void encoder_handler(sys_state_t state) { +//-------------------------------------------------- +//if (encoder_interrupt_busy == true) return; +//if (knob_x.delta_count != 0) +// { +// encoder_interrupt_busy = true; +// sign = knob_x.delta_count >= 0 ? 1.0f : -1.0f; +// plan_line_data_t plan_data_x = +// { +// 0 +// }; +// plan_data.feed_rate = 30000.0f; +// protocol_buffer_synchronize(); +// sync_position(); +// // Get current position. +// system_convert_array_steps_to_mpos(origin.values, sys.position); +// target.x = origin.x + sign * 25.0f; +// knob_x.prev_count = knob_x.count; +// mc_line(target.values, &plan_data_x); +// } +//if (knob_z.delta_count != 0) +// { +// encoder_interrupt_busy = true; +// sign = knob_z.delta_count >= 0 ? 1.0f : -1.0f; +// plan_line_data_t plan_data_z = +// { +// 0 +// }; +// plan_data_z.feed_rate = 30000.0f; +// protocol_buffer_synchronize(); +// sync_position(); +// // Get current position. +// system_convert_array_steps_to_mpos(origin.values, sys.position); +// target.z = origin.z + sign * 25.0f; +// knob_z.prev_count = knob_z.count; +// mc_line(target.values, &plan_data_z); +// } +//encoder_interrupt_busy = false; +//-------------------------------------------------- +knob_x.prev_count = knob_x.count; +knob_z.prev_count = knob_z.count; if (encoder_interrupt_busy == true) return; -if (knob_x.delta_count != 0) +if ((knob_x.delta_count != 0) || (knob_z.delta_count != 0)) { encoder_interrupt_busy = true; - sign = knob_x.delta_count >= 0 ? 1.0f : -1.0f; + knob_x.sign = knob_x.delta_count >= 0 ? 1.0f : -1.0f; + knob_z.sign = knob_z.delta_count >= 0 ? 1.0f : -1.0f; plan_line_data_t plan_data = { 0 @@ -379,12 +383,53 @@ if (knob_x.delta_count != 0) sync_position(); // Get current position. system_convert_array_steps_to_mpos(origin.values, sys.position); - target.x = origin.x + sign * 10.0f; - knob_x.prev_count = knob_x.count; - encoder_interrupt_busy = false; + if (knob_x.delta_count != 0) target.x = origin.x + knob_x.sign * 5.0f; + if (knob_z.delta_count != 0) target.z = origin.z + knob_z.sign * 5.0f; + mc_line(target.values, &plan_data); + encoder_interrupt_busy = false; } +} +static void blink_led(sys_state_t state) +{ +// static bool led_on = false; +static uint32_t ms = 0; + +if (hal.get_elapsed_ticks() >= ms) + { + ms = hal.get_elapsed_ticks() + 1000; //ms + +// HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_6); // Green +// HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_7); // Blue + HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_8); // Red + +// Alternative Method +// static bool led_on = false; +// static uint32_t ms = 0; +// +// if (hal.get_elapsed_ticks() >= ms) +// { +// ms = hal.get_elapsed_ticks() + 500; //ms +// led_on = !led_on; +// if (led_on) +// GPIOC->ODR |= GPIO_PIN_13; +// else +// GPIOC->ODR &= ~GPIO_PIN_13; +// } + + // temp - to be deleted +// target.x = sys.position[0] + 20.0f; +// target.y = sys.position[1]; +// hal.stream.write(uitoa(x)); +// hal.stream.write(","); +// hal.stream.write(uitoa(y)); +// hal.stream.write(ASCII_EOL); +// mc_line(target.values, &plan_data); + + } +//protocol_enqueue_rt_command(encoder_handler); +on_execute_realtime(state); } void led_init(void) From 6b00e09d27423d48f70f1e0f5f6c884be6431d19 Mon Sep 17 00:00:00 2001 From: atlesg Date: Fri, 21 Jan 2022 16:45:32 +0700 Subject: [PATCH 10/10] Update f4mpv5_hosoi.c --- Src/f4mpv5_hosoi.c | 127 +++++++++++++++++++++------------------------ 1 file changed, 60 insertions(+), 67 deletions(-) diff --git a/Src/f4mpv5_hosoi.c b/Src/f4mpv5_hosoi.c index ed4d8307..80b2fb26 100644 --- a/Src/f4mpv5_hosoi.c +++ b/Src/f4mpv5_hosoi.c @@ -7,7 +7,7 @@ #include "f4mpv4_hosoi.h" -TIM_HandleTypeDef htim2, htim3, htim5; +TIM_HandleTypeDef htim2, htim3, htim4; UART_HandleTypeDef huart4; static on_report_options_ptr on_report_options; @@ -76,9 +76,9 @@ void TIM3_IRQHandler(void) { HAL_TIM_IRQHandler(&htim3); } -//void TIM5_IRQHandler(void) +//void TIM4_IRQHandler(void) //{ -//HAL_TIM_IRQHandler(&htim5); +//HAL_TIM_IRQHandler(&htim4); //} void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim_encoder) @@ -104,7 +104,7 @@ if (htim_encoder->Instance == TIM2) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; GPIO_InitStruct.Alternate = GPIO_AF1_TIM2; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - HAL_NVIC_SetPriority(TIM2_IRQn, 2, 0); + HAL_NVIC_SetPriority(TIM2_IRQn, 0, 0); HAL_NVIC_EnableIRQ(TIM2_IRQn); } else if (htim_encoder->Instance == TIM3) @@ -117,35 +117,19 @@ else if (htim_encoder->Instance == TIM3) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; GPIO_InitStruct.Alternate = GPIO_AF2_TIM3; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - HAL_NVIC_SetPriority(TIM3_IRQn, 3, 0); + HAL_NVIC_SetPriority(TIM3_IRQn, 0, 0); HAL_NVIC_EnableIRQ(TIM3_IRQn); } -//else if (htim_encoder->Instance == TIM5) +//else if (htim_encoder->Instance == TIM4) // { -// /* USER CODE BEGIN TIM5_MspInit 0 */ -// -// /* USER CODE END TIM5_MspInit 0 */ -// /* Peripheral clock enable */ -// __HAL_RCC_TIM5_CLK_ENABLE(); -// -// __HAL_RCC_GPIOA_CLK_ENABLE(); -// /**TIM5 GPIO Configuration -// PA0-WKUP ------> TIM5_CH1 -// PA1 ------> TIM5_CH2 -// */ -// GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1; +// __HAL_RCC_TIM4_CLK_ENABLE(); +// __HAL_RCC_GPIOB_CLK_ENABLE(); +// GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7; // GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; // GPIO_InitStruct.Pull = GPIO_NOPULL; // GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; -// GPIO_InitStruct.Alternate = GPIO_AF2_TIM5; -// HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); -// -// /* TIM5 interrupt Init */ -// HAL_NVIC_SetPriority(TIM5_IRQn, 0, 0); -// HAL_NVIC_EnableIRQ(TIM5_IRQn); -// /* USER CODE BEGIN TIM5_MspInit 1 */ -// -// /* USER CODE END TIM5_MspInit 1 */ +// GPIO_InitStruct.Alternate = GPIO_AF2_TIM4; +// HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); // } } @@ -165,11 +149,10 @@ else if (htim_encoder->Instance == TIM3) HAL_GPIO_DeInit(GPIOB, GPIO_PIN_4 | GPIO_PIN_5); HAL_NVIC_DisableIRQ(TIM3_IRQn); } -//else if (htim_encoder->Instance == TIM5) +//else if (htim_encoder->Instance == TIM4) // { -// __HAL_RCC_TIM5_CLK_DISABLE(); -// HAL_GPIO_DeInit(GPIOA, GPIO_PIN_0 | GPIO_PIN_1); -// HAL_NVIC_DisableIRQ(TIM5_IRQn); +// __HAL_RCC_TIM4_CLK_DISABLE(); +// HAL_GPIO_DeInit(GPIOB, GPIO_PIN_6 | GPIO_PIN_7); // } } @@ -193,30 +176,37 @@ if (htim == knob_x.timer) { knob_x.counter = __HAL_TIM_GET_COUNTER(htim); knob_x.count = (int16_t) (knob_x.counter / 4); - // if (knob_x.delta_count != 0) // { // protocol_enqueue_rt_command(encoder_handler); // } } +if (htim == knob_y.timer) + { + knob_y.counter = __HAL_TIM_GET_COUNTER(htim); + knob_y.count = (int16_t) (knob_y.counter / 4); +// if (knob_z.delta_count != 0) +// { +// protocol_enqueue_rt_command(encoder_handler); +// } + } + if (htim == knob_z.timer) { - temp = __HAL_TIM_GET_COUNTER(htim); knob_z.counter = __HAL_TIM_GET_COUNTER(htim); knob_z.count = (int16_t) (knob_z.counter / 4); - // if (knob_z.delta_count != 0) // { // protocol_enqueue_rt_command(encoder_handler); // } } knob_x.delta_count = knob_x.count - knob_x.prev_count; +knob_y.delta_count = knob_y.count - knob_y.prev_count; knob_z.delta_count = knob_z.count - knob_z.prev_count; protocol_enqueue_rt_command(encoder_handler); - } static void encoder_init(void) @@ -257,32 +247,7 @@ if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) { Error_Handler(); } -//---------------------------------------- -//htim5.Instance = TIM5; -//htim5.Init.Prescaler = 0; -//htim5.Init.CounterMode = TIM_COUNTERMODE_UP; -//htim5.Init.Period = 4294967295; -//htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; -//htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; -//sConfig.EncoderMode = TIM_ENCODERMODE_TI12; -//sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; -//sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; -//sConfig.IC1Prescaler = TIM_ICPSC_DIV1; -//sConfig.IC1Filter = 0; -//sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; -//sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; -//sConfig.IC2Prescaler = TIM_ICPSC_DIV1; -//sConfig.IC2Filter = 0; -//if (HAL_TIM_Encoder_Init(&htim5, &sConfig) != HAL_OK) -// { -// Error_Handler(); -// } -//sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; -//sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; -//if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK) -// { -// Error_Handler(); -// } + //---------------------------------------- htim3.Instance = TIM3; htim3.Init.Prescaler = 0; @@ -309,11 +274,36 @@ if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) { Error_Handler(); } - +//---------------------------------------- +//htim4.Instance = TIM4; +//htim4.Init.Prescaler = 0; +//htim4.Init.CounterMode = TIM_COUNTERMODE_UP; +//htim4.Init.Period = 65535; +//htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; +//htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; +//sConfig.EncoderMode = TIM_ENCODERMODE_TI1; +//sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; +//sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; +//sConfig.IC1Prescaler = TIM_ICPSC_DIV1; +//sConfig.IC1Filter = 0; +//sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; +//sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; +//sConfig.IC2Prescaler = TIM_ICPSC_DIV1; +//sConfig.IC2Filter = 0; +//if (HAL_TIM_Encoder_Init(&htim4, &sConfig) != HAL_OK) +// { +// Error_Handler(); +// } +//sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; +//sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; +//if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) +// { +// Error_Handler(); +// } //---------------------------------------- knob_x.timer = &htim2; -//knob_y.timer = &htim5; -knob_z.timer = &htim3; +knob_y.timer = &htim3; +knob_z.timer = &htim4; for (uint8_t i = 0; i < NUMBER_OF_ENCODER; i++) { knob_ptr[i]->count = 0; @@ -322,8 +312,8 @@ for (uint8_t i = 0; i < NUMBER_OF_ENCODER; i++) knob_ptr[i]->prev_count = 0; } HAL_TIM_Encoder_Start_IT(knob_x.timer, TIM_CHANNEL_ALL); -//HAL_TIM_Encoder_Start_IT(knob_y.timer, TIM_CHANNEL_ALL); -HAL_TIM_Encoder_Start_IT(knob_z.timer, TIM_CHANNEL_ALL); +HAL_TIM_Encoder_Start_IT(knob_y.timer, TIM_CHANNEL_ALL); +//HAL_TIM_Encoder_Start_IT(knob_z.timer, TIM_CHANNEL_ALL); } static void encoder_handler(sys_state_t state) @@ -367,12 +357,14 @@ static void encoder_handler(sys_state_t state) //encoder_interrupt_busy = false; //-------------------------------------------------- knob_x.prev_count = knob_x.count; +knob_y.prev_count = knob_y.count; knob_z.prev_count = knob_z.count; if (encoder_interrupt_busy == true) return; -if ((knob_x.delta_count != 0) || (knob_z.delta_count != 0)) +if ((knob_x.delta_count != 0)|| (knob_y.delta_count != 0) || (knob_z.delta_count != 0)) { encoder_interrupt_busy = true; knob_x.sign = knob_x.delta_count >= 0 ? 1.0f : -1.0f; + knob_y.sign = knob_y.delta_count >= 0 ? 1.0f : -1.0f; knob_z.sign = knob_z.delta_count >= 0 ? 1.0f : -1.0f; plan_line_data_t plan_data = { @@ -384,6 +376,7 @@ if ((knob_x.delta_count != 0) || (knob_z.delta_count != 0)) // Get current position. system_convert_array_steps_to_mpos(origin.values, sys.position); if (knob_x.delta_count != 0) target.x = origin.x + knob_x.sign * 5.0f; + if (knob_y.delta_count != 0) target.y = origin.y + knob_y.sign * 5.0f; if (knob_z.delta_count != 0) target.z = origin.z + knob_z.sign * 5.0f; mc_line(target.values, &plan_data);