From 5b4c8676d6c81e11199e4b037bb359166d5b06d9 Mon Sep 17 00:00:00 2001
From: leo <leo.lemaire007@orange.fr>
Date: Fri, 1 Sep 2023 18:38:25 +0200
Subject: [PATCH] use 1 timer per unit

---
 Core/Inc/PSE_unit.h             |   7 +-
 Core/Inc/pse_stepper_planer.h   |  10 +-
 Core/Inc/stm32f1xx_it.h         |   3 +
 Core/Src/PSE_unit_edit_screen.c |   8 +-
 Core/Src/home_screen.c          |   2 +-
 Core/Src/main.c                 | 164 ++++++++++++++++++++++++++++++--
 Core/Src/pse_stepper_planer.c   | 110 ++++++++++++---------
 Core/Src/stm32f1xx_hal_msp.c    |  88 ++++++++++++++++-
 Core/Src/stm32f1xx_it.c         |  45 +++++++++
 PSE.ioc                         |  29 ++++--
 10 files changed, 394 insertions(+), 72 deletions(-)

diff --git a/Core/Inc/PSE_unit.h b/Core/Inc/PSE_unit.h
index 5d0e822..9b90cad 100644
--- a/Core/Inc/PSE_unit.h
+++ b/Core/Inc/PSE_unit.h
@@ -20,8 +20,9 @@ typedef struct{
 typedef enum {PORT_X, PORT_Y, PORT_Z, PORT_E} pse_ports;
 
 typedef struct{
-  uint32_t step_itvl;
-  uint8_t step_max;
+	uint16_t tim_presc;
+	uint16_t tim_period;
+
   uint32_t tick_counter;
   int32_t steps_counter;
   uint8_t stopAtHome;
@@ -32,6 +33,8 @@ typedef struct{
   uint16_t DIR_GPIO_Pin;
   GPIO_TypeDef* STEP_GPIO_Port;
   uint16_t STEP_GPIO_Pin;
+
+  TIM_HandleTypeDef* tim;
 } pse_stepper_conf;
 
 typedef struct{
diff --git a/Core/Inc/pse_stepper_planer.h b/Core/Inc/pse_stepper_planer.h
index 6fd2b82..a4d3fe1 100644
--- a/Core/Inc/pse_stepper_planer.h
+++ b/Core/Inc/pse_stepper_planer.h
@@ -10,23 +10,23 @@
 
 #include "PSE_unit.h"
 
-#define PSE_STEPPER_INTERRUPT_RATE 20000 // 64Mhz (Presc 64, count 50) : 20kHz
+#define PSE_STEPPER_TIMER_CLOCK 64 // 64Mhz
 #define PSE_STEPPER_SCREW_PITCH 1000 // M6 : 1000µm / rotation
 #define PSE_STEPPER_STEPS_PER_ROTATION 3200 // NEMA17 200 steps/rotation with 16x microstepping
+#define PSE_STEPPER_JOG_PRESC 64 // jog step frequency 50kHz
+#define PSE_STEPPER_JOG_PERIOD 25 // 64Mhz / 64 / 10 / 2
 
 void pse_sp_start_axis(pse_stepper_conf* conf);
 void pse_sp_stop_axis(pse_stepper_conf* conf);
 void pse_sp_set_dir(pse_stepper_conf* conf, int dir);
 int pse_sp_get_dir(pse_stepper_conf* conf);
-void pse_stepper_planer_tick(pse_unit* units, uint8_t units_num);
-
+void pse_stepper_planer_tick(pse_unit* unit);
+void pse_sp_jog_speed(pse_stepper_conf* conf, int status);
 
 void pse_sp_set_dir_all(pse_unit* units, int unit_num, int dir);
 void pse_sp_start_all(pse_unit* units, int unit_num);
 void pse_sp_stop_all(pse_unit* units, int unit_num);
 
-void pse_sp_set_timer(TIM_HandleTypeDef* tim);
-
 void pse_stepper_planer_compute_sps(pse_unit* unit);
 
 #endif /* INC_PSE_STEPPER_PLANER_H_ */
diff --git a/Core/Inc/stm32f1xx_it.h b/Core/Inc/stm32f1xx_it.h
index bb97afb..71599af 100644
--- a/Core/Inc/stm32f1xx_it.h
+++ b/Core/Inc/stm32f1xx_it.h
@@ -55,7 +55,10 @@ void SVC_Handler(void);
 void DebugMon_Handler(void);
 void PendSV_Handler(void);
 void SysTick_Handler(void);
+void TIM2_IRQHandler(void);
+void TIM3_IRQHandler(void);
 void TIM4_IRQHandler(void);
+void TIM5_IRQHandler(void);
 void DMA2_Channel1_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 
diff --git a/Core/Src/PSE_unit_edit_screen.c b/Core/Src/PSE_unit_edit_screen.c
index 7831a51..e4c594d 100644
--- a/Core/Src/PSE_unit_edit_screen.c
+++ b/Core/Src/PSE_unit_edit_screen.c
@@ -158,12 +158,12 @@ static void jog_forward_button_handler(lv_event_t* e){
 	pse_unit* unit = lv_event_get_user_data(e);
 
 	if(code == LV_EVENT_PRESSED) {
-		unit->stepper_conf->step_max = 1;
+		pse_sp_jog_speed(unit->stepper_conf, 1);
 		pse_sp_set_dir(unit->stepper_conf, 0);
 		pse_sp_start_axis(unit->stepper_conf);
 	}
 	else if(code == LV_EVENT_RELEASED){
-		unit->stepper_conf->step_max = 0;
+		pse_sp_jog_speed(unit->stepper_conf, 0);
 		pse_sp_stop_axis(unit->stepper_conf);
 	}
 }
@@ -173,12 +173,12 @@ static void jog_backward_button_handler(lv_event_t* e){
 	pse_unit* unit = lv_event_get_user_data(e);
 
 	if(code == LV_EVENT_PRESSED) {
-		unit->stepper_conf->step_max = 1;
+		pse_sp_jog_speed(unit->stepper_conf, 1);
 		pse_sp_set_dir(unit->stepper_conf, 1);
 		pse_sp_start_axis(unit->stepper_conf);
 	}
 	else if(code == LV_EVENT_RELEASED){
-		unit->stepper_conf->step_max = 0;
+		pse_sp_jog_speed(unit->stepper_conf, 0);
 		pse_sp_stop_axis(unit->stepper_conf);
 	}
 }
diff --git a/Core/Src/home_screen.c b/Core/Src/home_screen.c
index c37ed59..5a287aa 100644
--- a/Core/Src/home_screen.c
+++ b/Core/Src/home_screen.c
@@ -71,7 +71,7 @@ static void unit_home_handler(lv_event_t * e){
 		pse_stepper_conf* c = unit->stepper_conf;
 		if(c->steps_counter == 0) return;
 		c->stopAtHome = 1;
-		c->step_max = 1;
+		pse_sp_jog_speed(c, 1);
 		pse_sp_set_dir(c, c->steps_counter < 0);
 		pse_sp_start_axis(c);
 	}
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 8420fe7..30edde9 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -55,7 +55,10 @@
 /* Private variables ---------------------------------------------------------*/
 SD_HandleTypeDef hsd;
 
+TIM_HandleTypeDef htim2;
+TIM_HandleTypeDef htim3;
 TIM_HandleTypeDef htim4;
+TIM_HandleTypeDef htim5;
 
 UART_HandleTypeDef huart1;
 
@@ -98,7 +101,7 @@ pse_stepper_conf pse_stepper_confs[PSE_STEPPER_NUM] = {
 	.EN_GPIO_Pin = X_STEPPER_EN_Pin,
 	.STEP_GPIO_Port = X_STEPPER_STEP_GPIO_Port,
 	.STEP_GPIO_Pin = X_STEPPER_STEP_Pin,
-	.step_max = 69,
+	.tim = &htim2,
   },
   [PSE_Y_STEPPER] = {
     .DIR_GPIO_Port = Y_STEPPER_DIR_GPIO_Port,
@@ -107,6 +110,7 @@ pse_stepper_conf pse_stepper_confs[PSE_STEPPER_NUM] = {
 	.EN_GPIO_Pin = Y_STEPPER_EN_Pin,
 	.STEP_GPIO_Port = Y_STEPPER_STEP_GPIO_Port,
 	.STEP_GPIO_Pin = Y_STEPPER_STEP_Pin,
+	.tim = &htim3,
   },
   [PSE_Z_STEPPER] = {
     .DIR_GPIO_Port = Z_STEPPER_DIR_GPIO_Port,
@@ -115,6 +119,7 @@ pse_stepper_conf pse_stepper_confs[PSE_STEPPER_NUM] = {
 	.EN_GPIO_Pin = Z_STEPPER_EN_Pin,
 	.STEP_GPIO_Port = Z_STEPPER_STEP_GPIO_Port,
 	.STEP_GPIO_Pin = Z_STEPPER_STEP_Pin,
+	.tim = &htim4,
   },
   [PSE_E_STEPPER] = {
     .DIR_GPIO_Port = E_STEPPER_DIR_GPIO_Port,
@@ -123,6 +128,7 @@ pse_stepper_conf pse_stepper_confs[PSE_STEPPER_NUM] = {
 	.EN_GPIO_Pin = E_STEPPER_EN_Pin,
 	.STEP_GPIO_Port = E_STEPPER_STEP_GPIO_Port,
 	.STEP_GPIO_Pin = E_STEPPER_STEP_Pin,
+	.tim = &htim5,
   },
 };
 /* USER CODE END PV */
@@ -133,8 +139,11 @@ static void MX_GPIO_Init(void);
 static void MX_DMA_Init(void);
 static void MX_FSMC_Init(void);
 static void MX_USART1_UART_Init(void);
-static void MX_TIM4_Init(void);
 static void MX_SDIO_SD_Init(void);
+static void MX_TIM2_Init(void);
+static void MX_TIM3_Init(void);
+static void MX_TIM4_Init(void);
+static void MX_TIM5_Init(void);
 /* USER CODE BEGIN PFP */
 #ifdef __GNUC__
 #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
@@ -216,6 +225,9 @@ int main(void)
   MX_TIM4_Init();
   MX_FATFS_Init();
   MX_SDIO_SD_Init();
+  MX_TIM2_Init();
+  MX_TIM3_Init();
+  MX_TIM5_Init();
   /* USER CODE BEGIN 2 */
   // Reset the LCD to start clean
   HAL_GPIO_WritePin(LCD_RST_GPIO_Port, LCD_RST_Pin, GPIO_PIN_RESET);
@@ -279,9 +291,6 @@ int main(void)
   // Load saved units
   load_units(pse_units, pse_syringes, pse_stepper_confs, PSE_UNITS_NUM);
 
-  // Set timer used for step interrupts
-  pse_sp_set_timer(&htim4);
-
   // Load the home screen
   Home_Screen_Gen(pse_units, PSE_UNITS_NUM);
   /* USER CODE END 2 */
@@ -366,6 +375,96 @@ static void MX_SDIO_SD_Init(void)
 
 }
 
+/**
+  * @brief TIM2 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM2_Init(void)
+{
+
+  /* USER CODE BEGIN TIM2_Init 0 */
+
+  /* USER CODE END TIM2_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM2_Init 1 */
+
+  /* USER CODE END TIM2_Init 1 */
+  htim2.Instance = TIM2;
+  htim2.Init.Prescaler = 0;
+  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim2.Init.Period = 65535;
+  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM2_Init 2 */
+
+  /* USER CODE END TIM2_Init 2 */
+
+}
+
+/**
+  * @brief TIM3 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM3_Init(void)
+{
+
+  /* USER CODE BEGIN TIM3_Init 0 */
+
+  /* USER CODE END TIM3_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM3_Init 1 */
+
+  /* USER CODE END TIM3_Init 1 */
+  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;
+  if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM3_Init 2 */
+
+  /* USER CODE END TIM3_Init 2 */
+
+}
+
 /**
   * @brief TIM4 Initialization Function
   * @param None
@@ -385,9 +484,9 @@ static void MX_TIM4_Init(void)
 
   /* USER CODE END TIM4_Init 1 */
   htim4.Instance = TIM4;
-  htim4.Init.Prescaler = 64;
+  htim4.Init.Prescaler = 0;
   htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim4.Init.Period = 50;
+  htim4.Init.Period = 65535;
   htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
   if (HAL_TIM_Base_Init(&htim4) != HAL_OK)
@@ -411,6 +510,51 @@ static void MX_TIM4_Init(void)
 
 }
 
+/**
+  * @brief TIM5 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM5_Init(void)
+{
+
+  /* USER CODE BEGIN TIM5_Init 0 */
+
+  /* USER CODE END TIM5_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM5_Init 1 */
+
+  /* USER CODE END TIM5_Init 1 */
+  htim5.Instance = TIM5;
+  htim5.Init.Prescaler = 0;
+  htim5.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim5.Init.Period = 65535;
+  htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim5) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM5_Init 2 */
+
+  /* USER CODE END TIM5_Init 2 */
+
+}
+
 /**
   * @brief USART1 Initialization Function
   * @param None
@@ -631,7 +775,11 @@ static void MX_FSMC_Init(void)
 
 /* USER CODE BEGIN 4 */
 void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim){
-  pse_stepper_planer_tick(pse_units, PSE_UNITS_NUM);
+	for(int i = 0; i < PSE_UNITS_NUM; i++){
+		if(htim == pse_units[i].stepper_conf->tim){
+			pse_stepper_planer_tick(&pse_units[i]);
+		}
+	}
 }
 
 void XferCpltCallback(DMA_HandleTypeDef *hdma){
diff --git a/Core/Src/pse_stepper_planer.c b/Core/Src/pse_stepper_planer.c
index e81ed59..0c283b9 100644
--- a/Core/Src/pse_stepper_planer.c
+++ b/Core/Src/pse_stepper_planer.c
@@ -7,50 +7,29 @@
 
 #include <stdint.h>
 
+#include "lvgl.h"
+
 #include "PSE_unit.h"
 
 #include "pse_stepper_planer.h"
 
-static int units_running = 0; // counter of currently running units, to disable interrupts when unnecessary
-static TIM_HandleTypeDef* htim; // main interrupt timer handle
+void pse_stepper_planer_tick(pse_unit* unit){
+	pse_stepper_conf* c = unit->stepper_conf;
 
-void pse_stepper_planer_tick(pse_unit* units, uint8_t units_num){
-	for(int i = 0; i < units_num; i++){
-		pse_stepper_conf* c = units[i].stepper_conf;
-
-		int state;
-		if(c->step_max){
-			c->tick_counter %= 2;
-			state = c->tick_counter;
-		}
-		else{
-			c->tick_counter %= c->step_itvl;
-			state = c->tick_counter == 0;
-		}
-		c->tick_counter++;
-		HAL_GPIO_WritePin(c->STEP_GPIO_Port, c->STEP_GPIO_Pin, state);
-		c->steps_counter += (pse_sp_get_dir(c)?1:-1) * (state?1:0);
-		if(c->stopAtHome && c->steps_counter == 0){
-			pse_sp_stop_axis(c);
-		}
+	HAL_GPIO_TogglePin(c->STEP_GPIO_Port, c->STEP_GPIO_Pin);
+	c->steps_counter += (pse_sp_get_dir(c)?1:-1) * HAL_GPIO_ReadPin(c->STEP_GPIO_Port, c->STEP_GPIO_Pin);
+	if(c->stopAtHome && c->steps_counter == 0){
+		pse_sp_jog_speed(c, 0);
+		pse_sp_stop_axis(c);
 	}
 }
 
 void pse_sp_start_axis(pse_stepper_conf* conf){
-	if(HAL_GPIO_ReadPin(conf->EN_GPIO_Port, conf->EN_GPIO_Pin) == GPIO_PIN_SET){
-		if(units_running == 0)
-			HAL_TIM_Base_Start_IT(htim);
-		units_running++;
-	}
-
 	HAL_GPIO_WritePin(conf->EN_GPIO_Port, conf->EN_GPIO_Pin, GPIO_PIN_RESET);
+	HAL_TIM_Base_Start_IT(conf->tim);
 }
 void pse_sp_stop_axis(pse_stepper_conf* conf){
-	if(HAL_GPIO_ReadPin(conf->EN_GPIO_Port, conf->EN_GPIO_Pin) == GPIO_PIN_RESET){
-		units_running--;
-		if(units_running == 0)
-			HAL_TIM_Base_Stop_IT(htim);
-	}
+	HAL_TIM_Base_Stop_IT(conf->tim);
 	HAL_GPIO_WritePin(conf->EN_GPIO_Port, conf->EN_GPIO_Pin, GPIO_PIN_SET);
 }
 
@@ -62,12 +41,56 @@ int pse_sp_get_dir(pse_stepper_conf* conf){
 	return HAL_GPIO_ReadPin(conf->DIR_GPIO_Port, conf->DIR_GPIO_Pin);
 }
 
+// https://en.wikipedia.org/wiki/Integer_square_root#Using_only_integer_division
+static uint16_t int_sqrt(uint32_t s){
+	if (s <= 1)
+		return s;
+
+	uint32_t x0 = s / 2;
+
+	uint32_t x1 = (x0 + s / x0) / 2;
+
+	while (x1 < x0){
+		x0 = x1;
+		x1 = (x0 + s / x0) / 2;
+	}
+	return x0;
+}
+
 void pse_stepper_planer_compute_sps(pse_unit* unit){
 	pse_stepper_conf* c = unit->stepper_conf;
 	// magic constants are used to approximate pi and other multiplicatives constant as a rationnal
-	uint64_t numerator = (uint64_t)1 * unit->syringe->diameter * unit->syringe->diameter * PSE_STEPPER_INTERRUPT_RATE * PSE_STEPPER_SCREW_PITCH;
-	uint64_t denominator = (uint64_t)21220209 * unit->flow * PSE_STEPPER_STEPS_PER_ROTATION;
-	c->step_itvl = numerator / denominator;
+	uint64_t numerator = (uint64_t)14323 * unit->syringe->diameter * unit->syringe->diameter * PSE_STEPPER_TIMER_CLOCK * PSE_STEPPER_SCREW_PITCH;
+	uint64_t denominator = (uint64_t)607887 * unit->flow * PSE_STEPPER_STEPS_PER_ROTATION;
+	uint64_t res = numerator / denominator;
+	if(res == 0){
+		lv_obj_t * mbox1 = lv_msgbox_create(NULL, "Error", "Invalid configuration entered (period = 0)", NULL, true);
+		lv_obj_center(mbox1);
+		return;
+	}
+	else if(res >= ((uint64_t)1 << 32)){
+		lv_obj_t * mbox1 = lv_msgbox_create(NULL, "Error", "Invalid configuration entered (period > 2^32)", NULL, true);
+		lv_obj_center(mbox1);
+		return;
+	}
+
+	if(res < 65536){
+		c->tim_presc = 0;
+		c->tim_period = res - 1;
+		__HAL_TIM_SET_AUTORELOAD(c->tim, c->tim_period);
+		__HAL_TIM_SET_PRESCALER(c->tim, c->tim_presc);
+	}
+	else{
+		uint16_t sqrt = int_sqrt(res);
+		c->tim_presc = sqrt - 1;
+		c->tim_period = sqrt;
+		__HAL_TIM_SET_AUTORELOAD(c->tim, c->tim_period);
+		__HAL_TIM_SET_PRESCALER(c->tim, c->tim_presc);
+	}
+	lv_obj_t * mbox1 = lv_msgbox_create(NULL, "Info", "aaa", NULL, true);
+	lv_obj_t * mboxtxt = lv_msgbox_get_text(mbox1);
+	lv_label_set_text_fmt(mboxtxt, "res : %llu\npresc : %d\nperiod : %d", res, c->tim_presc, c->tim_period);
+	lv_obj_center(mbox1);
 }
 
 void pse_sp_set_dir_all(pse_unit* units, int units_num, int dir){
@@ -77,12 +100,9 @@ void pse_sp_set_dir_all(pse_unit* units, int units_num, int dir){
 }
 
 void pse_sp_start_all(pse_unit* units, int units_num){
-	HAL_TIM_Base_Start_IT(htim);
-
 	for(int i = 0; i < units_num; i++){
 		if(units[i].enabled){
 			pse_sp_start_axis(units[i].stepper_conf);
-			units_running++;
 		}
 	}
 }
@@ -91,11 +111,15 @@ void pse_sp_stop_all(pse_unit* units, int units_num){
 		if(units[i].enabled)
 			pse_sp_stop_axis(units[i].stepper_conf);
 	}
-
-	HAL_TIM_Base_Stop_IT(htim);
-	units_running = 0;
 }
 
-void pse_sp_set_timer(TIM_HandleTypeDef* tim){
-	htim = tim;
+void pse_sp_jog_speed(pse_stepper_conf* conf, int status){
+	if(status){
+		__HAL_TIM_SET_AUTORELOAD(conf->tim, PSE_STEPPER_JOG_PERIOD);
+		__HAL_TIM_SET_PRESCALER(conf->tim, PSE_STEPPER_JOG_PRESC);
+	}
+	else{
+		__HAL_TIM_SET_AUTORELOAD(conf->tim, conf->tim_period);
+		__HAL_TIM_SET_PRESCALER(conf->tim, conf->tim_presc);
+	}
 }
diff --git a/Core/Src/stm32f1xx_hal_msp.c b/Core/Src/stm32f1xx_hal_msp.c
index ee5d2fa..2ad644c 100644
--- a/Core/Src/stm32f1xx_hal_msp.c
+++ b/Core/Src/stm32f1xx_hal_msp.c
@@ -158,7 +158,35 @@ void HAL_SD_MspDeInit(SD_HandleTypeDef* hsd)
 */
 void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base)
 {
-  if(htim_base->Instance==TIM4)
+  if(htim_base->Instance==TIM2)
+  {
+  /* USER CODE BEGIN TIM2_MspInit 0 */
+
+  /* USER CODE END TIM2_MspInit 0 */
+    /* Peripheral clock enable */
+    __HAL_RCC_TIM2_CLK_ENABLE();
+    /* TIM2 interrupt Init */
+    HAL_NVIC_SetPriority(TIM2_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM2_IRQn);
+  /* USER CODE BEGIN TIM2_MspInit 1 */
+
+  /* USER CODE END TIM2_MspInit 1 */
+  }
+  else if(htim_base->Instance==TIM3)
+  {
+  /* USER CODE BEGIN TIM3_MspInit 0 */
+
+  /* USER CODE END TIM3_MspInit 0 */
+    /* Peripheral clock enable */
+    __HAL_RCC_TIM3_CLK_ENABLE();
+    /* TIM3 interrupt Init */
+    HAL_NVIC_SetPriority(TIM3_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM3_IRQn);
+  /* USER CODE BEGIN TIM3_MspInit 1 */
+
+  /* USER CODE END TIM3_MspInit 1 */
+  }
+  else if(htim_base->Instance==TIM4)
   {
   /* USER CODE BEGIN TIM4_MspInit 0 */
 
@@ -172,6 +200,20 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base)
 
   /* USER CODE END TIM4_MspInit 1 */
   }
+  else if(htim_base->Instance==TIM5)
+  {
+  /* USER CODE BEGIN TIM5_MspInit 0 */
+
+  /* USER CODE END TIM5_MspInit 0 */
+    /* Peripheral clock enable */
+    __HAL_RCC_TIM5_CLK_ENABLE();
+    /* 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 */
+  }
 
 }
 
@@ -183,7 +225,35 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base)
 */
 void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base)
 {
-  if(htim_base->Instance==TIM4)
+  if(htim_base->Instance==TIM2)
+  {
+  /* USER CODE BEGIN TIM2_MspDeInit 0 */
+
+  /* USER CODE END TIM2_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM2_CLK_DISABLE();
+
+    /* TIM2 interrupt DeInit */
+    HAL_NVIC_DisableIRQ(TIM2_IRQn);
+  /* USER CODE BEGIN TIM2_MspDeInit 1 */
+
+  /* USER CODE END TIM2_MspDeInit 1 */
+  }
+  else if(htim_base->Instance==TIM3)
+  {
+  /* USER CODE BEGIN TIM3_MspDeInit 0 */
+
+  /* USER CODE END TIM3_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM3_CLK_DISABLE();
+
+    /* TIM3 interrupt DeInit */
+    HAL_NVIC_DisableIRQ(TIM3_IRQn);
+  /* USER CODE BEGIN TIM3_MspDeInit 1 */
+
+  /* USER CODE END TIM3_MspDeInit 1 */
+  }
+  else if(htim_base->Instance==TIM4)
   {
   /* USER CODE BEGIN TIM4_MspDeInit 0 */
 
@@ -197,6 +267,20 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base)
 
   /* USER CODE END TIM4_MspDeInit 1 */
   }
+  else if(htim_base->Instance==TIM5)
+  {
+  /* USER CODE BEGIN TIM5_MspDeInit 0 */
+
+  /* USER CODE END TIM5_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM5_CLK_DISABLE();
+
+    /* TIM5 interrupt DeInit */
+    HAL_NVIC_DisableIRQ(TIM5_IRQn);
+  /* USER CODE BEGIN TIM5_MspDeInit 1 */
+
+  /* USER CODE END TIM5_MspDeInit 1 */
+  }
 
 }
 
diff --git a/Core/Src/stm32f1xx_it.c b/Core/Src/stm32f1xx_it.c
index e6a2b92..84c82df 100644
--- a/Core/Src/stm32f1xx_it.c
+++ b/Core/Src/stm32f1xx_it.c
@@ -57,7 +57,10 @@
 
 /* External variables --------------------------------------------------------*/
 extern DMA_HandleTypeDef hdma_memtomem_dma2_channel1;
+extern TIM_HandleTypeDef htim2;
+extern TIM_HandleTypeDef htim3;
 extern TIM_HandleTypeDef htim4;
+extern TIM_HandleTypeDef htim5;
 /* USER CODE BEGIN EV */
 
 /* USER CODE END EV */
@@ -200,6 +203,34 @@ void SysTick_Handler(void)
 /* please refer to the startup file (startup_stm32f1xx.s).                    */
 /******************************************************************************/
 
+/**
+  * @brief This function handles TIM2 global interrupt.
+  */
+void TIM2_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM2_IRQn 0 */
+
+  /* USER CODE END TIM2_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim2);
+  /* USER CODE BEGIN TIM2_IRQn 1 */
+
+  /* USER CODE END TIM2_IRQn 1 */
+}
+
+/**
+  * @brief This function handles TIM3 global interrupt.
+  */
+void TIM3_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM3_IRQn 0 */
+
+  /* USER CODE END TIM3_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim3);
+  /* USER CODE BEGIN TIM3_IRQn 1 */
+
+  /* USER CODE END TIM3_IRQn 1 */
+}
+
 /**
   * @brief This function handles TIM4 global interrupt.
   */
@@ -213,6 +244,20 @@ void TIM4_IRQHandler(void)
   /* USER CODE END TIM4_IRQn 1 */
 }
 
+/**
+  * @brief This function handles TIM5 global interrupt.
+  */
+void TIM5_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM5_IRQn 0 */
+
+  /* USER CODE END TIM5_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim5);
+  /* USER CODE BEGIN TIM5_IRQn 1 */
+
+  /* USER CODE END TIM5_IRQn 1 */
+}
+
 /**
   * @brief This function handles DMA2 channel1 global interrupt.
   */
diff --git a/PSE.ioc b/PSE.ioc
index 27f3e56..6b57cfc 100644
--- a/PSE.ioc
+++ b/PSE.ioc
@@ -22,14 +22,17 @@ Mcu.CPN=STM32F103VET6
 Mcu.Family=STM32F1
 Mcu.IP0=DMA
 Mcu.IP1=FATFS
+Mcu.IP10=TIM5
+Mcu.IP11=USART1
 Mcu.IP2=FSMC
 Mcu.IP3=NVIC
 Mcu.IP4=RCC
 Mcu.IP5=SDIO
 Mcu.IP6=SYS
-Mcu.IP7=TIM4
-Mcu.IP8=USART1
-Mcu.IPNb=9
+Mcu.IP7=TIM2
+Mcu.IP8=TIM3
+Mcu.IP9=TIM4
+Mcu.IPNb=12
 Mcu.Name=STM32F103V(C-D-E)Tx
 Mcu.Package=LQFP100
 Mcu.Pin0=PE2
@@ -74,13 +77,16 @@ Mcu.Pin43=PE0
 Mcu.Pin44=PE1
 Mcu.Pin45=VP_FATFS_VS_SDIO
 Mcu.Pin46=VP_SYS_VS_Systick
-Mcu.Pin47=VP_TIM4_VS_ClockSourceINT
+Mcu.Pin47=VP_TIM2_VS_ClockSourceINT
+Mcu.Pin48=VP_TIM3_VS_ClockSourceINT
+Mcu.Pin49=VP_TIM4_VS_ClockSourceINT
 Mcu.Pin5=PE7
+Mcu.Pin50=VP_TIM5_VS_ClockSourceINT
 Mcu.Pin6=PE8
 Mcu.Pin7=PE9
 Mcu.Pin8=PE10
 Mcu.Pin9=PE11
-Mcu.PinsNb=48
+Mcu.PinsNb=51
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F103VETx
@@ -97,7 +103,10 @@ NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false
+NVIC.TIM2_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
+NVIC.TIM3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.TIM4_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
+NVIC.TIM5_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 PA10.Mode=Asynchronous
 PA10.Signal=USART1_RX
@@ -312,15 +321,21 @@ SDIO.HardwareFlowControl=SDIO_HARDWARE_FLOW_CONTROL_DISABLE
 SDIO.IPParameters=ClockDiv,HardwareFlowControl
 TIM4.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
 TIM4.IPParameters=Prescaler,Period,AutoReloadPreload
-TIM4.Period=50
-TIM4.Prescaler=64
+TIM4.Period=65535
+TIM4.Prescaler=0
 USART1.IPParameters=VirtualMode
 USART1.VirtualMode=VM_ASYNC
 VP_FATFS_VS_SDIO.Mode=SDIO
 VP_FATFS_VS_SDIO.Signal=FATFS_VS_SDIO
 VP_SYS_VS_Systick.Mode=SysTick
 VP_SYS_VS_Systick.Signal=SYS_VS_Systick
+VP_TIM2_VS_ClockSourceINT.Mode=Internal
+VP_TIM2_VS_ClockSourceINT.Signal=TIM2_VS_ClockSourceINT
+VP_TIM3_VS_ClockSourceINT.Mode=Internal
+VP_TIM3_VS_ClockSourceINT.Signal=TIM3_VS_ClockSourceINT
 VP_TIM4_VS_ClockSourceINT.Mode=Internal
 VP_TIM4_VS_ClockSourceINT.Signal=TIM4_VS_ClockSourceINT
+VP_TIM5_VS_ClockSourceINT.Mode=Internal
+VP_TIM5_VS_ClockSourceINT.Signal=TIM5_VS_ClockSourceINT
 board=custom
 isbadioc=false