添加python 算法仿真代码

yincheng.zhong

9 天以前 b9aca884f88abdda860d5c62be841a9e21ce68a1

 STM32H743/FML/pwm_ctrol.c

@@ -10,6 +10,7 @@
#include "DBG.h"
#include "Uart.h"
#include "HIDO_Util.h"
#include "SBUS.h"

#define STATE_WAIT_RISING   0
#define STATE_WAIT_FALLING  1
@@ -21,7 +22,7 @@



// 设置所有电机到指定占空比
// �������е����ָ��ռ�ձ�
void set_all_pwm(uint16_t duty)
{
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty);
@@ -34,45 +35,45 @@
    __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); // 后右
    __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); // 后右
    __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]
// ӳ�亯������ [-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（前进/后退）
// ���õ�� 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
    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（左转/右转）
// ����ת�� 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
    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));
@@ -89,40 +90,79 @@

        if (capture_state == STATE_WAIT_RISING)
        {
            // 当前是上升沿 → 记录并切换到等待下降沿
            // ��ǰ�������� �� ��¼���л����ȴ��½���
            rising_time = current_time;
            // 切换为下降沿触发
            htim->Instance->CCER &= ~TIM_CCER_CC1P; // 上升沿
            htim->Instance->CCER |= TIM_CCER_CC1NP; // 加上 NP 表示非反相？实际应使用极性控制函数
            // �л�Ϊ�½��ش���
            htim->Instance->CCER &= ~TIM_CCER_CC1P; // ������
            htim->Instance->CCER |= TIM_CCER_CC1NP; // ���� NP ��ʾ�Ƿ��ࣿʵ��Ӧʹ�ü��Կ��ƺ���

            // 更推荐使用 HAL 函数设置极性
            // ���Ƽ�ʹ�� 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)  // 超过最大合理值
            if (pulse_width > 65535)  // ����������ֵ
            {
                pulse_width = 0; // 或者标记为无效
                pulse_width = 0; // ���߱��Ϊ��Ч
            }

            //printf("High Pulse Width: %lu μs\n", pulse_width);
            //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)
@@ -131,10 +171,10 @@
{
    if (htim->Instance == TIM4)
    {
        // 只在长时间无响应时才重置状态
        // ֻ�ڳ�ʱ������Ӧʱ������״̬
        static uint32_t last_reset_ms = 0;

        if (HAL_GetTick() - last_reset_ms > 100)  // 100ms 超时
        if (HAL_GetTick() - last_reset_ms > 100)  // 100ms ��ʱ
        {
            capture_state = 0;
            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
@@ -154,30 +194,30 @@

        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)  // 超过最大合理值
            // ��ֹ��������ֹ�����
            if (pulse_width_us > 65535)  // ����������ֵ
            {
                pulse_width_us = 0; // 或者标记为无效
                pulse_width_us = 0; // ���߱��Ϊ��Ч
            }

            printf("Falling Edge: %lu\r\n", falling_time);
            printf("Pulse Width: %lu μs\r\n", pulse_width_us);
            printf("Pulse Width: %lu ��s\r\n", pulse_width_us);

            // 重新设置为上升沿，等待下一个周期
            // ��������Ϊ�����أ��ȴ���һ������
            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
            capture_state = 0;
        }