From 21a8f757d00620fb777ed4a139c802136e5cb095 Mon Sep 17 00:00:00 2001 From: Benjamin Date: Tue, 2 Jun 2020 11:53:59 +0200 Subject: [PATCH 1/3] PWM Steering fixed --- Inc/config.h | 2 +- Src/control.c | 118 ++++++++++++++++++++++---------------------------- 2 files changed, 53 insertions(+), 67 deletions(-) diff --git a/Inc/config.h b/Inc/config.h index b8ffd51..3609f26 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -298,7 +298,7 @@ #define PWM_CH2_MIN -1000 // (-1000 - 0) #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. #define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 - #define STEER_COEFFICIENT 0 // 0.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 0 = 0.0 * 2^14. If you do not want any steering, set it to 0. + #define STEER_COEFFICIENT 16384 // 0.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 0 = 0.0 * 2^14. If you do not want any steering, set it to 0. // #define SUPPORT_BUTTONS // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! // #define INVERT_R_DIRECTION // #define INVERT_L_DIRECTION diff --git a/Src/control.c b/Src/control.c index b0e9022..78205a2 100644 --- a/Src/control.c +++ b/Src/control.c @@ -88,109 +88,95 @@ void PPM_Init(void) { #ifdef CONTROL_PWM uint16_t pwm_captured_ch1_value = 500; uint16_t pwm_captured_ch2_value = 500; -uint32_t pwm_timeout_ch1 = 0; -uint32_t pwm_timeout_ch2 = 0; +uint32_t pwm_timeout = 0; -void PWM_ISR_CH1_Callback(void) { - // Dummy loop with 16 bit count wrap around - uint16_t rc_signal = TIM3->CNT; - TIM3->CNT = 0; - - if (IN_RANGE(rc_signal, 900, 2100)){ - timeout = 0; - pwm_timeout_ch1 = 0; - pwm_captured_ch1_value = CLAMP(rc_signal, 1000, 2000) - 1000; +int PWM_Signal_Correct(int x, int max, int min) { + int outVal = 0; + if(x > -PWM_DEADBAND && x < PWM_DEADBAND) { + outVal = 0; + } else if(x > 0) { + outVal = (float)CLAMP(x-PWM_DEADBAND, 0, max - PWM_DEADBAND) / (max - PWM_DEADBAND) * 1000; + } else { + outVal = 0 - ((float)CLAMP(x+PWM_DEADBAND, min + PWM_DEADBAND, 0) / (min + PWM_DEADBAND) * 1000); } + return outVal; } - -void PWM_ISR_CH2_Callback(void) { +void PWM_ISR_CH1_Callback() { // Dummy loop with 16 bit count wrap around uint16_t rc_signal = TIM2->CNT; TIM2->CNT = 0; if (IN_RANGE(rc_signal, 900, 2100)){ timeout = 0; - pwm_timeout_ch2 = 0; + pwm_timeout = 0; + pwm_captured_ch1_value = CLAMP(rc_signal, 1000, 2000) - 1000; + } +} + +void PWM_ISR_CH2_Callback() { + // Dummy loop with 16 bit count wrap around + uint16_t rc_signal = TIM2->CNT; + TIM2->CNT = 0; + + if (IN_RANGE(rc_signal, 900, 2100)){ + timeout = 0; + pwm_timeout = 0; pwm_captured_ch2_value = CLAMP(rc_signal, 1000, 2000) - 1000; } } // SysTick executes once each ms -void PWM_SysTick_Callback(void) { - pwm_timeout_ch1++; - pwm_timeout_ch2++; - // Stop after 500 ms without PWM signal - if(pwm_timeout_ch1 > 500) { - pwm_captured_ch1_value = 500; - pwm_timeout_ch1 = 500; // limit the timeout to max timeout value of 500 ms - } - if(pwm_timeout_ch2 > 500) { +void PWM_SysTick_Callback() { + pwm_timeout++; + // Stop after 500 ms without PPM signal + if(pwm_timeout > 500) { + //pwm_captured_ch1_value = 500; pwm_captured_ch2_value = 500; - pwm_timeout_ch2 = 500; // limit the timeout to max timeout value of 500 ms + pwm_timeout = 0; } } -void PWM_Init(void) { +void PWM_Init() { + // PWM Timer (TIM2) + __HAL_RCC_TIM2_CLK_ENABLE(); + TimHandle.Instance = TIM2; + TimHandle.Init.Period = UINT16_MAX; + TimHandle.Init.Prescaler = (SystemCoreClock/DELAY_TIM_FREQUENCY_US)-1;; + TimHandle.Init.ClockDivision = 0; + TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; + HAL_TIM_Base_Init(&TimHandle); + + // Channel 1 (steering) GPIO_InitTypeDef GPIO_InitStruct2; // Configure GPIO pin : PA2 - GPIO_InitStruct2.Pin = GPIO_PIN_2; - GPIO_InitStruct2.Mode = GPIO_MODE_IT_RISING_FALLING; - GPIO_InitStruct2.Speed = GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct2.Pull = GPIO_PULLDOWN; + GPIO_InitStruct2.Pin = GPIO_PIN_2; + GPIO_InitStruct2.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct2.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct2.Pull = GPIO_PULLDOWN; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct2); - __HAL_RCC_TIM3_CLK_ENABLE(); - TimHandle2.Instance = TIM3; - TimHandle2.Init.Period = UINT16_MAX; - TimHandle2.Init.Prescaler = (SystemCoreClock/DELAY_TIM_FREQUENCY_US)-1;; - TimHandle2.Init.ClockDivision = 0; - TimHandle2.Init.CounterMode = TIM_COUNTERMODE_UP; - HAL_TIM_Base_Init(&TimHandle2); - // EXTI interrupt init HAL_NVIC_SetPriority(EXTI2_IRQn, 0, 0); HAL_NVIC_EnableIRQ(EXTI2_IRQn); - HAL_TIM_Base_Start(&TimHandle2); + // Channel 2 (speed) GPIO_InitTypeDef GPIO_InitStruct; /*Configure GPIO pin : PA3 */ - GPIO_InitStruct.Pin = GPIO_PIN_3; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.Pull = GPIO_PULLDOWN; + GPIO_InitStruct.Pin = GPIO_PIN_3; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Pull = GPIO_PULLDOWN; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); - __HAL_RCC_TIM2_CLK_ENABLE(); - TimHandle.Instance = TIM2; - TimHandle.Init.Period = UINT16_MAX; - TimHandle.Init.Prescaler = (SystemCoreClock/DELAY_TIM_FREQUENCY_US)-1;; - TimHandle.Init.ClockDivision = 0; - TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; - HAL_TIM_Base_Init(&TimHandle); - /* EXTI interrupt init*/ HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0); HAL_NVIC_EnableIRQ(EXTI3_IRQn); + + // Start timer HAL_TIM_Base_Start(&TimHandle); - - #ifdef SUPPORT_BUTTONS - /*Configure GPIO pin : PB10 */ - GPIO_InitStruct.Pin = BUTTON1_RIGHT_PIN; - GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM; - GPIO_InitStruct.Pull = GPIO_PULLUP; - HAL_GPIO_Init(BUTTON1_RIGHT_PORT, &GPIO_InitStruct); - - /*Configure GPIO pin : PB11 */ - GPIO_InitStruct2.Pin = BUTTON2_RIGHT_PIN; - GPIO_InitStruct2.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct2.Speed = GPIO_SPEED_FREQ_MEDIUM; - GPIO_InitStruct2.Pull = GPIO_PULLUP; - HAL_GPIO_Init(BUTTON2_RIGHT_PORT, &GPIO_InitStruct2); - #endif } #endif From b95307f260b98eb41cd09246990f8537d684a9bc Mon Sep 17 00:00:00 2001 From: EmanuelFeru Date: Tue, 2 Jun 2020 13:04:10 +0200 Subject: [PATCH 2/3] Update control.c - removed the `PWM_signal_Correct(..)` function, because it is already existing here https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC/blob/34c6e4d610181b0bf6677bb50b3a3dcf69ad2b88/Src/util.c#L844-L862 - added (void) to the functions - limited `pwm_timeout` to 500 instead of 0, ottherwise the timeout won't work properly, because it will will reset every time. --- Src/control.c | 48 ++++++++++++++++++------------------------------ 1 file changed, 18 insertions(+), 30 deletions(-) diff --git a/Src/control.c b/Src/control.c index 78205a2..70ac4cd 100644 --- a/Src/control.c +++ b/Src/control.c @@ -90,19 +90,7 @@ uint16_t pwm_captured_ch1_value = 500; uint16_t pwm_captured_ch2_value = 500; uint32_t pwm_timeout = 0; -int PWM_Signal_Correct(int x, int max, int min) { - int outVal = 0; - if(x > -PWM_DEADBAND && x < PWM_DEADBAND) { - outVal = 0; - } else if(x > 0) { - outVal = (float)CLAMP(x-PWM_DEADBAND, 0, max - PWM_DEADBAND) / (max - PWM_DEADBAND) * 1000; - } else { - outVal = 0 - ((float)CLAMP(x+PWM_DEADBAND, min + PWM_DEADBAND, 0) / (min + PWM_DEADBAND) * 1000); - } - return outVal; -} - -void PWM_ISR_CH1_Callback() { +void PWM_ISR_CH1_Callback(void) { // Dummy loop with 16 bit count wrap around uint16_t rc_signal = TIM2->CNT; TIM2->CNT = 0; @@ -114,7 +102,7 @@ void PWM_ISR_CH1_Callback() { } } -void PWM_ISR_CH2_Callback() { +void PWM_ISR_CH2_Callback(void) { // Dummy loop with 16 bit count wrap around uint16_t rc_signal = TIM2->CNT; TIM2->CNT = 0; @@ -127,34 +115,34 @@ void PWM_ISR_CH2_Callback() { } // SysTick executes once each ms -void PWM_SysTick_Callback() { +void PWM_SysTick_Callback(void) { pwm_timeout++; // Stop after 500 ms without PPM signal if(pwm_timeout > 500) { //pwm_captured_ch1_value = 500; pwm_captured_ch2_value = 500; - pwm_timeout = 0; + pwm_timeout = 500; } } -void PWM_Init() { +void PWM_Init(void) { // PWM Timer (TIM2) __HAL_RCC_TIM2_CLK_ENABLE(); - TimHandle.Instance = TIM2; - TimHandle.Init.Period = UINT16_MAX; - TimHandle.Init.Prescaler = (SystemCoreClock/DELAY_TIM_FREQUENCY_US)-1;; - TimHandle.Init.ClockDivision = 0; - TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; + TimHandle.Instance = TIM2; + TimHandle.Init.Period = UINT16_MAX; + TimHandle.Init.Prescaler = (SystemCoreClock/DELAY_TIM_FREQUENCY_US)-1;; + TimHandle.Init.ClockDivision = 0; + TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; HAL_TIM_Base_Init(&TimHandle); // Channel 1 (steering) GPIO_InitTypeDef GPIO_InitStruct2; // Configure GPIO pin : PA2 - GPIO_InitStruct2.Pin = GPIO_PIN_2; - GPIO_InitStruct2.Mode = GPIO_MODE_IT_RISING_FALLING; - GPIO_InitStruct2.Speed = GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct2.Pull = GPIO_PULLDOWN; + GPIO_InitStruct2.Pin = GPIO_PIN_2; + GPIO_InitStruct2.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct2.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct2.Pull = GPIO_PULLDOWN; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct2); // EXTI interrupt init @@ -165,10 +153,10 @@ void PWM_Init() { // Channel 2 (speed) GPIO_InitTypeDef GPIO_InitStruct; /*Configure GPIO pin : PA3 */ - GPIO_InitStruct.Pin = GPIO_PIN_3; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.Pull = GPIO_PULLDOWN; + GPIO_InitStruct.Pin = GPIO_PIN_3; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Pull = GPIO_PULLDOWN; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); /* EXTI interrupt init*/ From 22411e1851c8f8b6b17dc4d8ab9e0cc7cdc2dd68 Mon Sep 17 00:00:00 2001 From: EmanuelFeru Date: Tue, 2 Jun 2020 14:42:14 +0200 Subject: [PATCH 3/3] update PWM timeout - updated the `pwm_timeout` and uncommented `pwm_captured_ch1_value` as discussed with @benjaf --- Inc/config.h | 2 +- Src/control.c | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Inc/config.h b/Inc/config.h index 3609f26..75c5422 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -298,7 +298,7 @@ #define PWM_CH2_MIN -1000 // (-1000 - 0) #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. #define SPEED_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14 - #define STEER_COEFFICIENT 16384 // 0.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 0 = 0.0 * 2^14. If you do not want any steering, set it to 0. + #define STEER_COEFFICIENT 16384 // 1.0f [-] fixdt(1,16,14) higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14. If you do not want any steering, set it to 0. // #define SUPPORT_BUTTONS // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! // #define INVERT_R_DIRECTION // #define INVERT_L_DIRECTION diff --git a/Src/control.c b/Src/control.c index 70ac4cd..dcda286 100644 --- a/Src/control.c +++ b/Src/control.c @@ -119,9 +119,9 @@ void PWM_SysTick_Callback(void) { pwm_timeout++; // Stop after 500 ms without PPM signal if(pwm_timeout > 500) { - //pwm_captured_ch1_value = 500; + pwm_captured_ch1_value = 500; pwm_captured_ch2_value = 500; - pwm_timeout = 500; + pwm_timeout = 0; } }