diff --git a/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx b/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx index 0adcb37..1b0b016 100644 Binary files a/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx and b/01_Matlab/BLDCmotorControl_FOC_R2017b_fixdt.slx differ diff --git a/Inc/config.h b/Inc/config.h index c45c561..23b7866 100644 --- a/Inc/config.h +++ b/Inc/config.h @@ -287,9 +287,9 @@ #define PWM_DEADBAND 100 // How much of the center position is considered 'center' (100 = values -100 to 100 are considered 0) // Min / Max values of each channel (use DEBUG to determine these values) #define PWM_CH1_MAX 1000 // (0 - 1000) - #define PWM_CH1_MIN -1000 // (-1000 - 0) - #define PWM_CH2_MAX 500 // (0 - 1000) - #define PWM_CH2_MIN -800 // (-1000 - 0) + #define PWM_CH1_MIN -1000 // (-1000 - 0) + #define PWM_CH2_MAX 1000 // (0 - 1000) + #define PWM_CH2_MIN -1000 // (-1000 - 0) // right sensor board cable. Only read once during startup #define BUTTONS_RIGHT // use right sensor board cable for button inputs. Disable DEBUG_SERIAL_USART3! #define FILTER 6553 // 0.1f [-] fixdt(0,16,16) lower value == softer filter [0, 65535] = [0.0 - 1.0]. diff --git a/Inc/defines.h b/Inc/defines.h index b8bbcd6..024b63e 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -170,8 +170,6 @@ void PPM_ISR_Callback(void); void PWM_Init(void); //void PWM_ISR_CH1_Callback(void); void PWM_ISR_CH2_Callback(void); -void PWM_SysTick_Callback(void); -int PWM_Signal_Correct(int16_t x, int16_t max, int16_t min); // Sideboard definitions #define LED1_SET (0x01) diff --git a/Inc/util.h b/Inc/util.h index 3e864c8..e39b618 100644 --- a/Inc/util.h +++ b/Inc/util.h @@ -73,6 +73,7 @@ void poweroffPressCheck(void); // Read Command Function void readCommand(void); +int PWM_Signal_Correct(int16_t u, int16_t min, int16_t max); // Sideboard functions void sideboardLeds(uint8_t *leds); diff --git a/Src/control.c b/Src/control.c index d80ba35..a85b3df 100644 --- a/Src/control.c +++ b/Src/control.c @@ -123,25 +123,14 @@ uint32_t pwm_timeout = 0; #define IN_RANGE(x, low, up) (((x) >= (low)) && ((x) <= (up))) -int PWM_Signal_Correct(int16_t x, int16_t max, int16_t 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) { // Dummy loop with 16 bit count wrap around uint16_t rc_signal = TIM3->CNT; TIM3->CNT = 0; - if (IN_RANGE(rc_signal, 900, 2100)){ + // The interval check below should be larger than the feasible PWM interval of ~[500, 2500] ms + if (IN_RANGE(rc_signal, 200, 4000)){ timeout = 0; pwm_timeout = 0; pwm_captured_ch1_value = CLAMP(rc_signal, 1000, 2000) - 1000; @@ -154,7 +143,8 @@ void PWM_ISR_CH2_Callback(void) { uint16_t rc_signal = TIM2->CNT; TIM2->CNT = 0; - if (IN_RANGE(rc_signal, 900, 2100)){ + // The interval check below should be larger than the feasible PWM interval of ~[900, 2100] ms + if (IN_RANGE(rc_signal, 200, 3000)){ timeout = 0; pwm_timeout = 0; pwm_captured_ch2_value = CLAMP(rc_signal, 1000, 2000) - 1000; @@ -164,11 +154,11 @@ void PWM_ISR_CH2_Callback(void) { // SysTick executes once each ms void PWM_SysTick_Callback(void) { pwm_timeout++; - // Stop after 500 ms without PPM signal + // Stop after 500 ms without PWM signal if(pwm_timeout > 500) { //pwm_captured_ch1_value = 500; pwm_captured_ch2_value = 500; - pwm_timeout = 0; + pwm_timeout = 500; // limit the timeout to max timeout value of 500 ms } } @@ -177,18 +167,18 @@ void PWM_Init(void) { /* 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.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; + TimHandle2.Init.CounterMode = TIM_COUNTERMODE_UP; HAL_TIM_Base_Init(&TimHandle2); // EXTI interrupt init @@ -201,18 +191,18 @@ void PWM_Init(void) { 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; + 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*/ diff --git a/Src/main.c b/Src/main.c index 446ee58..bade613 100644 --- a/Src/main.c +++ b/Src/main.c @@ -251,8 +251,8 @@ int main(void) { // speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), INPUT_MIN, INPUT_MA); mixerFcn(speed << 4, steer << 4, &speedR, &speedL); // This function implements the equations above - // ####### SET OUTPUTS (if the target change is less than +/- 50) ####### - if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) { + // ####### SET OUTPUTS (if the target change is less than +/- 100) ####### + if ((speedL > lastSpeedL-100 && speedL < lastSpeedL+100) && (speedR > lastSpeedR-100 && speedR < lastSpeedR+100) && timeout < TIMEOUT) { #ifdef INVERT_R_DIRECTION pwmr = speedR; #else diff --git a/Src/util.c b/Src/util.c index 314f7f2..9fb8e82 100644 --- a/Src/util.c +++ b/Src/util.c @@ -649,8 +649,8 @@ void readCommand(void) { #endif #ifdef CONTROL_PWM - cmd1 = 0; // CLAMP(PWM_Signal_Correct((pwm_captured_ch1_value - 500) * 2, PWM_CH1_MAX, PWM_CH1_MIN), INPUT_MIN, INPUT_MAX); - cmd2 = CLAMP(PWM_Signal_Correct((pwm_captured_ch2_value - 500) * 2, PWM_CH2_MAX, PWM_CH2_MIN), INPUT_MIN, INPUT_MAX); + cmd1 = 0; // CLAMP(PWM_Signal_Correct((pwm_captured_ch1_value - 500) * 2, PWM_CH1_MIN, PWM_CH1_MAX), INPUT_MIN, INPUT_MAX); + cmd2 = CLAMP(PWM_Signal_Correct((pwm_captured_ch2_value - 500) * 2, PWM_CH2_MIN, PWM_CH2_MAX), INPUT_MIN, INPUT_MAX); #endif #ifdef CONTROL_ADC @@ -811,7 +811,6 @@ void readCommand(void) { if (main_loop_counter % 30 == 0) { HAL_UART_DMAStop(&huart3); HAL_UART_Receive_DMA(&huart3, (uint8_t *)&Sideboard_Rnew, sizeof(Sideboard_Rnew)); - Sideboard_Rnew.start = 0xFFFF; // Change the Start Frame to avoid entering again here if no data is received } } timeoutFlagSerial = timeoutFlagSerial_R; @@ -840,6 +839,22 @@ void readCommand(void) { } + /* + * PWM Signal Correction + * This function realizes a dead-band around 0 and scales the input within a min and a max + */ +int PWM_Signal_Correct(int16_t u, int16_t min, int16_t max) { + int outVal = 0; + if(u > -PWM_DEADBAND && u < PWM_DEADBAND) { + outVal = 0; + } else if(u > 0) { + outVal = (INPUT_MAX * CLAMP(u - PWM_DEADBAND, 0, max - PWM_DEADBAND)) / (max - PWM_DEADBAND); + } else { + outVal = (INPUT_MIN * CLAMP(u + PWM_DEADBAND, min + PWM_DEADBAND, 0)) / (min + PWM_DEADBAND); + } + return outVal; +} + /* =========================== Sideboard Functions =========================== */