#include "stdio.h"
|
#include "stdarg.h"
|
#include "string.h"
|
#include "pwm_ctrol.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)
|
{
|
__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(&htim1, TIM_CHANNEL_4, duty);
|
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, duty);
|
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, duty);
|
__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, 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_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
|
