| | |
|---|
| | | #include "stdarg.h" |
|---|
| | | #include "string.h" |
|---|
| | | #include "pwm_ctrol.h" |
|---|
| | | #include "mainex.h" |
|---|
| | | #include "bluetooth.h" |
|---|
| | | #include "stm32h7xx_it.h" |
|---|
| | | #include "stm32h7xx_hal.h" |
|---|
| | | #include "main.h" |
|---|
| | | #include "mainex.h" |
|---|
| | | #include "DBG.h" |
|---|
| | | #include "Uart.h" |
|---|
| | | #include "HIDO_Util.h" |
|---|
| | | #include "SBUS.h" |
|---|
| | | |
|---|
| | | #define STATE_WAIT_RISING 0 |
|---|
| | | #define STATE_WAIT_FALLING 1 |
|---|
| | | uint32_t rising_time = 0; |
|---|
| | | uint32_t falling_time = 0; |
|---|
| | | uint32_t pulse_width_us = 0; |
|---|
| | | uint8_t capture_state = STATE_WAIT_RISING;; |
|---|
| | | |
|---|
| | | |
|---|
| | | |
|---|
| | | |
|---|
| | | |
|---|
| | | // 设置所有电机到指定占空比 |
|---|
| | | void set_all_pwm(uint16_t duty) { |
|---|
| | | // �������е����ָ��ռ�ձ� |
|---|
| | | void set_all_pwm(uint16_t duty) |
|---|
| | | { |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty); |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, duty); |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty); |
|---|
| | |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, duty); |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 向左：前轮左转，后轮右转 |
|---|
| | | void set_pwm_left() { |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 1000); // 前左 |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 2000); // 前右 |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 2000); // 后左 |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 1000); // 后右 |
|---|
| | | // ����ǰ����ת��������ת |
|---|
| | | void set_pwm_left() |
|---|
| | | { |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 1000); // ǰ�� |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 2000); // ǰ�� |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 2000); // ���� |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 1000); // ���� |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 向右：前轮右转，后轮左转 |
|---|
| | | void set_pwm_right() { |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 2000); // 前左 |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 1000); // 前右 |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 1000); // 后左 |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 2000); // 后右 |
|---|
| | | // ���ң�ǰ����ת��������ת |
|---|
| | | void set_pwm_right() |
|---|
| | | { |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 2000); // ǰ�� |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 1000); // ǰ�� |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 1000); // ���� |
|---|
| | | __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 2000); // ���� |
|---|
| | | } |
|---|
| | | |
|---|
| | | |
|---|
| | | // ӳ�亯������ [-100,100] ӳ�䵽 [1000,2000] |
|---|
| | | uint32_t Map(int16_t input, int16_t in_min, int16_t in_max, uint32_t out_min, uint32_t out_max) |
|---|
| | | { |
|---|
| | | return (input - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
|---|
| | | } |
|---|
| | | // ���õ�� PWM��ǰ��/���ˣ� |
|---|
| | | void Set_Motor_PWM(int16_t speed) |
|---|
| | | { |
|---|
| | | static HIDO_UINT8 l_Motor[50]; |
|---|
| | | uint32_t pulse = Map(speed, -100, 100, 1000, 2000); // -100~100 �� 1000~2000 |
|---|
| | | __HAL_TIM_SetCompare(&MOTOR_TIM, MOTOR_CHANNEL, pulse); |
|---|
| | | HIDO_UtilSnprintf((HIDO_CHAR *)l_Motor, sizeof(l_Motor), "Motor cortrol:speed=%d,pulse=%d\r\n", speed,pulse); |
|---|
| | | Uart_Send(UART_ID_DBG, (HIDO_UINT8 *)l_Motor, strlen(l_Motor)); |
|---|
| | | } |
|---|
| | | |
|---|
| | | // ����ת�� PWM����ת/��ת�� |
|---|
| | | void Set_Steering_PWM(int16_t steer) |
|---|
| | | { |
|---|
| | | static HIDO_UINT8 l_Steering[50]; |
|---|
| | | uint32_t pulse = Map(steer, -100, 100, 1000, 2000); // -100~100 �� 1000~2000 |
|---|
| | | __HAL_TIM_SetCompare(&STEERING_TIM, STEERING_CHANNEL, pulse); |
|---|
| | | HIDO_UtilSnprintf((HIDO_CHAR *)l_Steering, sizeof(l_Steering), "Steering cortrol:steer=%d,pulse=%d\r\n", steer,pulse); |
|---|
| | | Uart_Send(UART_ID_DBG, (HIDO_UINT8 *)l_Steering, strlen(l_Steering)); |
|---|
| | | |
|---|
| | | } |
|---|
| | | |
|---|
| | | void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) |
|---|
| | | { |
|---|
| | | static HIDO_UINT8 l_pulse_width[20]; |
|---|
| | | |
|---|
| | | if (htim->Instance == TIM4) |
|---|
| | | { |
|---|
| | | uint32_t current_time = htim->Instance->CCR1; |
|---|
| | | |
|---|
| | | if (capture_state == STATE_WAIT_RISING) |
|---|
| | | { |
|---|
| | | // ��ǰ�������� �� ��¼���л����ȴ��½��� |
|---|
| | | rising_time = current_time; |
|---|
| | | // �л�Ϊ�½��ش��� |
|---|
| | | htim->Instance->CCER &= ~TIM_CCER_CC1P; // ������ |
|---|
| | | htim->Instance->CCER |= TIM_CCER_CC1NP; // ���� NP ��ʾ�Ƿ��ࣿʵ��Ӧʹ�ü��Կ��ƺ��� |
|---|
| | | |
|---|
| | | // ���Ƽ�ʹ�� HAL �������ü��� |
|---|
| | | __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING); |
|---|
| | | |
|---|
| | | capture_state = STATE_WAIT_FALLING; |
|---|
| | | } |
|---|
| | | else if (capture_state == STATE_WAIT_FALLING) |
|---|
| | | { |
|---|
| | | // ��ǰ���½��� �� ����ߵ�ƽ���� |
|---|
| | | uint32_t pulse_width = current_time - rising_time; |
|---|
| | | |
|---|
| | | if (pulse_width > 65535) // ����������ֵ |
|---|
| | | { |
|---|
| | | pulse_width = 0; // ���߱��Ϊ��Ч |
|---|
| | | } |
|---|
| | | |
|---|
| | | //printf("High Pulse Width: %lu ��s\n", pulse_width); |
|---|
| | | HIDO_UtilSnprintf((HIDO_CHAR *)l_pulse_width, sizeof(l_pulse_width), "pulse_width=%d\r\n", pulse_width); |
|---|
| | | Uart_Send(UART_ID_DBG, (HIDO_UINT8 *)l_pulse_width, strlen(l_pulse_width)); |
|---|
| | | // �л������ش��� |
|---|
| | | __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING); |
|---|
| | | capture_state = STATE_WAIT_RISING; |
|---|
| | | } |
|---|
| | | |
|---|
| | | // ����жϱ�־ |
|---|
| | | __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC1); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | uint32_t steering_pulse,motor_pulse; |
|---|
| | | void SBUS_Control_PWM(void) |
|---|
| | | { |
|---|
| | | ST_SBUSData sbusData; |
|---|
| | | |
|---|
| | | // 获取 SBUS 数据 |
|---|
| | | if (SBUS_GetData(&sbusData) != HIDO_TRUE) |
|---|
| | | { |
|---|
| | | return; // 没有有效数据 |
|---|
| | | } |
|---|
| | | |
|---|
| | | // CH0: 转向通道 (200-1800) -> (1000-2000us) |
|---|
| | | uint16_t ch0 = sbusData.m_au16Channels[0]; |
|---|
| | | if (ch0 < 200) ch0 = 200; |
|---|
| | | if (ch0 > 1800) ch0 = 1800; |
|---|
| | | steering_pulse = ((ch0 - 200) * 1000) / 1600 + 1000; |
|---|
| | | |
|---|
| | | // CH1: 行进通道 (200-1800) -> (1000-2000us) |
|---|
| | | uint16_t ch1 = sbusData.m_au16Channels[1]; |
|---|
| | | if (ch1 < 200) ch1 = 200; |
|---|
| | | if (ch1 > 1800) ch1 = 1800; |
|---|
| | | motor_pulse = ((ch1 - 200) * 1000) / 1600 + 1000; |
|---|
| | | |
|---|
| | | // 设置 PWM 输出 |
|---|
| | | __HAL_TIM_SetCompare(&STEERING_TIM, STEERING_CHANNEL, steering_pulse); |
|---|
| | | __HAL_TIM_SetCompare(&MOTOR_TIM, MOTOR_CHANNEL, motor_pulse); |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 直接设置脉宽（微秒） |
|---|
| | | void Set_Steering_Pulse(uint32_t pulse_us) |
|---|
| | | { |
|---|
| | | steering_pulse = pulse_us; |
|---|
| | | __HAL_TIM_SetCompare(&STEERING_TIM, STEERING_CHANNEL, steering_pulse); |
|---|
| | | } |
|---|
| | | |
|---|
| | | void Set_Motor_Pulse(uint32_t pulse_us) |
|---|
| | | { |
|---|
| | | motor_pulse = pulse_us; |
|---|
| | | __HAL_TIM_SetCompare(&MOTOR_TIM, MOTOR_CHANNEL, motor_pulse); |
|---|
| | | } |
|---|
| | | #if 0 |
|---|
| | | /** |
|---|
| | | * @brief Update Callback (for overflow protection) |
|---|
| | | */ |
|---|
| | | void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) |
|---|
| | | { |
|---|
| | | if (htim->Instance == TIM4) |
|---|
| | | { |
|---|
| | | // ֻ�ڳ�ʱ������Ӧʱ������״̬ |
|---|
| | | static uint32_t last_reset_ms = 0; |
|---|
| | | |
|---|
| | | if (HAL_GetTick() - last_reset_ms > 100) // 100ms ��ʱ |
|---|
| | | { |
|---|
| | | capture_state = 0; |
|---|
| | | __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING); |
|---|
| | | last_reset_ms = HAL_Get_Tick(); |
|---|
| | | printf("TIM4 Overflow Reset (timeout)\r\n"); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | #endif |
|---|
| | | #if 0 |
|---|
| | | void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) |
|---|
| | | { |
|---|
| | | if (htim->Instance == TIM4 && htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) |
|---|
| | | { |
|---|
| | | uint32_t current_value = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1); |
|---|
| | | |
|---|
| | | if (capture_state == 0) |
|---|
| | | { |
|---|
| | | // �����أ���¼��ʼʱ�� |
|---|
| | | rising_time = current_value; |
|---|
| | | printf("Rising Edge: %lu\r\n", rising_time); |
|---|
| | | |
|---|
| | | // �л�Ϊ�½��ؼ�� |
|---|
| | | __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING); |
|---|
| | | capture_state = 1; |
|---|
| | | } |
|---|
| | | else if (capture_state == 1) |
|---|
| | | { |
|---|
| | | // �½��أ��������� |
|---|
| | | falling_time = current_value; |
|---|
| | | pulse_width_us = falling_time - rising_time; |
|---|
| | | |
|---|
| | | // ��ֹ��������ֹ����� |
|---|
| | | if (pulse_width_us > 65535) // ����������ֵ |
|---|
| | | { |
|---|
| | | pulse_width_us = 0; // ���߱��Ϊ��Ч |
|---|
| | | } |
|---|
| | | |
|---|
| | | printf("Falling Edge: %lu\r\n", falling_time); |
|---|
| | | printf("Pulse Width: %lu ��s\r\n", pulse_width_us); |
|---|
| | | |
|---|
| | | // ��������Ϊ�����أ��ȴ���һ������ |
|---|
| | | __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING); |
|---|
| | | capture_state = 0; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | } |
|---|
| | | #endif |
|---|
| | | |
|---|
