张同学03 2025-03-14 18:43 采纳率: 50%
浏览 60
已结题

STM32及FreeRTOS的平衡小车

将STM32的裸机程序改为FreeRTOS的三个任务
裸机程序是在每次接收到6050的中断中执行。数据读取。 Pid输出PWM。进行控制。
FreeRTOS三个任务,分别为数据读取,pid控制。 OLED显示。
现在的问题是。改为FreeRTOS,我不知道是不是数据传递的问题。小车的车轮会来回左右转。疯狂转动,不会平稳。
如果是裸机的程序小车非常稳
到底是什么原因啊,如何改?

一下午又改了改
添加了消息队列修改优先级还是不行,PID也不会有什么问题吧(裸机程序中很平稳),但问什么输出的pwm如此高,角度显示只偏一点,轮子还是狂转?

**//rtos任务代码**
#include "main.h"
#include "timers.h"
int Bluetooth_data;

float Angle_turn = 0;
int L_Speed,R_Speed;
float pitch,roll,yaw;         //欧拉角
short aacx,aacy,aacz;        //加速度传感器原始数据
short gyrox,gyroy,gyroz;    //陀螺仪原始数据
u8 stop_flag=0;
extern int Moto1,Moto2;
extern u8 Fore,Back,Left,Right;

TaskHandle_t start_task_handle;
#define START_TASK_STACK 128
#define START_TASK_PRIORITY 1
void App_Task_Start(void *pvParameters);

/* 获取姿态数据的任务配置 */
TaskHandle_t data_task_handle;
#define DATA_TASK_STACK 384
#define DATA_TASK_PRIORITY 4
void App_Task_GetData(void *pvParameters);

/* 进行PID控制的任务配置 */
TaskHandle_t pid_task_handle;
#define PID_TASK_STACK 384
#define PID_TASK_PRIORITY 5
void App_Task_PID(void *pvParameters);

/* 进行oled显示的任务配置 */
TaskHandle_t display_task_handle;
#define DISPLAY_TASK_STACK 256
#define DISPLAY_TASK_PRIORITY 3
void App_Task_Display(void *pvParameters);

void App_Task_Init(void)
{
    /* 1. 创建启动任务 */
    xTaskCreate(
        (TaskFunction_t)App_Task_Start,
        (char *)"App_Task_Start",
        (configSTACK_DEPTH_TYPE)START_TASK_STACK,
        (void *)NULL,
        (UBaseType_t)START_TASK_PRIORITY,
        (TaskHandle_t *)&start_task_handle);
            
    /* 2. 启动调度器 */
    vTaskStartScheduler();
}

/**
 * @description: 启动任务:用于创建其他任务
 * @param {void *} pvParameters
 * @return {*}
 */
void App_Task_Start(void *pvParameters)
{        BaseType_t xReturn = pdPASS;
    taskENTER_CRITICAL();
    xReturn=xTaskCreate(
        (TaskFunction_t)App_Task_GetData,
        (char *)"App_Task_GetData",
        (configSTACK_DEPTH_TYPE)DATA_TASK_STACK,
        (void *)NULL,
        (UBaseType_t)DATA_TASK_PRIORITY,
        (TaskHandle_t *)&data_task_handle);
            if(xReturn==pdPASS)
            printf("GetData TASK SUCCEED!\r\n");
            
    xReturn=xTaskCreate(
        (TaskFunction_t)App_Task_PID,
        (char *)"App_Task_PID",
        (configSTACK_DEPTH_TYPE)PID_TASK_STACK,
        (void *)NULL,
        (UBaseType_t)PID_TASK_PRIORITY,
        (TaskHandle_t *)&pid_task_handle);
            if(xReturn==pdPASS)
            printf("PID TASK SUCCEED!\r\n");
            
    xReturn=xTaskCreate(
        (TaskFunction_t)App_Task_Display,
        (char *)"App_Task_Display",
        (configSTACK_DEPTH_TYPE)DISPLAY_TASK_STACK,
        (void *)NULL,
        (UBaseType_t)DISPLAY_TASK_PRIORITY,
        (TaskHandle_t *)&display_task_handle);
            if(xReturn==pdPASS)
            printf("OLED TASK SUCCEED!\r\n");
            
    vTaskDelete(NULL);
    taskEXIT_CRITICAL();
}

/**
 * @description: 获取姿态数据:角度、编码器值
 * @param {void} *pvParameters
 * @return {*}
 */
void App_Task_GetData(void *pvParameters)
{
//    TickType_t pxPreviousWakeTime = xTaskGetTickCount();
//        if (mpu_dmp_init() != 0) {  // 添加初始化状态检查
//            OLED_ShowString(1, 1, "L:");
//            OLED_Clear();}
//        else{OLED_ShowString(1, 1, "OK");
//            OLED_Clear();}
        
    while (1) {
        // 等待中断通知,阻塞直到数据就绪
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

        
        // 读取传感器数据(移至任务中)
        if (mpu_dmp_get_data(&pitch, &roll, &yaw) == 0) {
            MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);  
        } 
//                printf("A: %0.2f,%0.2f,%0.2f\r\n", pitch, roll, yaw);
        
        L_Speed = Read_Encoder(3);
        R_Speed = Read_Encoder(4);
    
        /* 获取完数据,通知pid控制任务 */
        xTaskNotifyGive(pid_task_handle);
//        vTaskDelayUntil(&pxPreviousWakeTime, 2);
    }
}

/**
 * @description: 进行PID控制的任务
 * @param {void} *pvParameters
 * @return {*}
 */
void App_Task_PID(void *pvParameters)
{    
    int Target=0;
//    TickType_t last_wake = xTaskGetTickCount();
  
    while(1)
    {
        /* 等待“获取数据任务”的通知 */
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
//            printf("Gyro: %d %d %d\r\n", gyrox, gyroy, gyroz);
//            printf("B: %0.2f,%0.2f,%0.2f\r\n", pitch, roll, yaw);
                pid.Pv = roll*10;                                                                                //角度*十倍                                            
                if(Turn_Off(pid.Pv))stop_flag=1;                                                    //检测是否小车异常,异常就停止
                else stop_flag=0;
        Moto1 = balance(pid.Pv) + Velocity(Target,L_Speed, R_Speed) ;//+ turn(gyroz, Angle_turn);        
        Moto2 = balance(pid.Pv) + Velocity(Target,L_Speed, R_Speed) ;//- turn(gyroz, Angle_turn);
        Xianfu_Pwm(); // 对PWM进行限幅
//                printf("PWM: %d %d \r\n", Moto1, Moto2);
        Set_Pwm(Moto1, Moto2); 
                xTaskNotifyGive(display_task_handle);
//                vTaskDelayUntil(&last_wake, 2);
    }
}

/**
 * @description: OLED显示任务
 * @param {void} *pvParameters
 * @return {*}
 */
void App_Task_Display(void *pvParameters)
{
//    TickType_t pxPreviousTime = xTaskGetTickCount();
    while(1)
    {
//            printf("C: %0.2f,%0.2f,%0.2f\r\n", pitch, roll, yaw);
            ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
        OLED_ShowString(1, 1, "L:");
        OLED_ShowSignedNum(1, 3, L_Speed, 2);
        OLED_ShowString(1, 9, "R:");
        OLED_ShowSignedNum(1, 11, R_Speed, 2);
        OLED_ShowString(2, 1, "x:");
        OLED_ShowSignedNum(2, 3, pitch, 2);
        OLED_ShowString(2, 9, "y:");
        OLED_ShowSignedNum(2, 11, roll, 2);
        OLED_ShowString(3, 1, "Blue:");
        OLED_ShowNum(3, 6, Bluetooth_data, 4);
        OLED_ShowString(4, 1, "BAT:");
        // OLED_ShowNum(4, 6, BAT, 4);
//        vTaskDelayUntil(&pxPreviousTime, 2);
    }
}

void EXTI9_5_IRQHandler(void) {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    if (EXTI_GetITStatus(EXTI_Line8) == SET) {
//            printf("PWM" );
        // 发送通知前检查任务句柄有效性
        if (data_task_handle != NULL) {
            xTaskNotifyFromISR(data_task_handle, 0, eNoAction, &xHigherPriorityTaskWoken);
        }
        EXTI_ClearITPendingBit(EXTI_Line8);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}


**//裸机主要代码**
int main(void)
{       
      OLED_Init();
     TIM3_Encoder_Init();
     TIM4_Encoder_Init();
     MOTOR_Init();
     TIM2_PWM_Init(7199,0);                           //PWM输出初始化
     MPU6050_EXTI_Init(); 
     MPU_Init();
     mpu_dmp_init();
     PID_Init();
    uart2_Init();

//    OLED_ShowString(1,1, "L_Speed:   RPM");
//    OLED_ShowString(2,1, "R_Speed:   RPM");
////    OLED_ShowString(3,1, "pitch:");
//    OLED_ShowString(3,1, "roll:");
//    OLED_ShowString(4,1, "yaw:");
//    
    
    
//    Moto1=60000;
//    Moto2=60000;
//    Set_Pwm(Moto1,Moto2); 
    while(1)
    {
      printf("%d,%d,%0.2f,%0.2f,%0.2f\n",Moto1,Moto2,pitch,roll,yaw);//这里打印了多少个就是多少个通道,上位机会识别
        My_XianShi();
    } 

}

void EXTI9_5_IRQHandler(void)
{
  if (EXTI_GetITStatus(EXTI_Line8)==SET)
  {
       YunDong();
      
    EXTI_ClearITPendingBit(EXTI_Line8);
  }



}



void YunDong(void)
{
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
        { 
            MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
        }
      pid.Pv = roll*10;                                                                                //角度*十倍                                            
            if(Turn_Off(pid.Pv))stop_flag=1;                                                    //检测是否小车异常,异常就停止
            else stop_flag=0;
            
            L_Speed=Read_Encoder(3);    
            R_Speed=Read_Encoder(4);
        
        
        // 2.将数据压入闭环控制中,计算出控制输出量
            
      /*前后*/
      if((Fore==0)&&(Back==0))Target=0;   // 未接收到前进后退指令->速度清零,稳在原地
      if(Fore==1)Target++;  // 前进1标志位拉高->需要前进
      if(Back==1)Target--;  // 
        Xianfu_Pwm();// 限幅
      
      /*左右*/
      if((Left==0)&&(Right==0))Angle_turn=0;
      if(Left==1)Angle_turn--;   // 左转
      if(Right==1)Angle_turn++;  // 右转
         Xianfu_Pwm();        // 限幅
      
//      /*转向约束*/
      if((Left==0)&&(Right==0))pid.Turn_Kd=0.1;    // 若无左右转向指令,则开启转向约束
      else if((Left==1)||(Right==1))pid.Turn_Kd=0;  // 若左右转向指令接收到,则去掉转向约束
//      Velocity_out=Velocity(Target,Encoder_Left,Encoder_Right); // 速度环
//      Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);              // 直立环
//            Turn_out=Turn(gyroz,Turn_Speed);
        
        
        
        
        
            Moto1 = balance(pid.Pv) +Velocity(Target,L_Speed,R_Speed)+turn(gyroz,Angle_turn);//;        
            Moto2 = balance(pid.Pv) + Velocity(Target,L_Speed,R_Speed)-turn(gyroz,Angle_turn);//;
            /*STM32输出给电机用于控制其姿态的PWM应为小车的直立环B_PWM与速度环V_PWM之和,
            如果有遥控左右则应该叠加相应的转向环的T_PWM分别控制左右电机。
            Mt1=BPwm+Ve Pwm-TPwm;∥计算左轮电机最终PWM
            Mt2=BPwm+Ve Pwm+TPwm:∥计算右轮电机最终PWM*/
            
            
            Xianfu_Pwm();                                                                                            //对PWM行限幅
            Set_Pwm(Moto1,Moto2);      
}


**//添加了消息队列,还是狂转**
#include "main.h"


/* 定义消息队列结构体 */
typedef struct {
    float gyroz;
      short gyrox;
    float roll;
    int L_Speed;
    int R_Speed;
} SensorData_t;

typedef struct {
//    float pitch;
    float roll;
    int L_Speed;
    int R_Speed;
    int Moto1;
    int Moto2;
    int Bluetooth_data;
} DisplayData_t;

//typedef struct {
//    int Bluetooth_data;
//} bluetoothData_t;

QueueHandle_t sensorQueue;  // GetData任务到PID任务的队列
QueueHandle_t displayQueue; // PID任务到Display任务的队列
QueueHandle_t bluetoothQueue; // 蓝牙数据队列






//int L_Speed,R_Speed;
//float pitch,roll,yaw;         //欧拉角
//short aacx,aacy,aacz;        //加速度传感器原始数据
//short gyrox,gyroy,gyroz;    //陀螺仪原始数据
int Bluetooth_data = 0;
u8 stop_flag=0;
//extern int Moto1,Moto2;


TaskHandle_t start_task_handle;
#define START_TASK_STACK 128
#define START_TASK_PRIORITY 1
void App_Task_Start(void *pvParameters);

/* 获取姿态数据的任务配置 */
TaskHandle_t data_task_handle;
#define DATA_TASK_STACK 384
#define DATA_TASK_PRIORITY 4
void App_Task_GetData(void *pvParameters);

/* 进行PID控制的任务配置 */
TaskHandle_t pid_task_handle;
#define PID_TASK_STACK 384
#define PID_TASK_PRIORITY 5
void App_Task_PID(void *pvParameters);

/* 进行oled显示的任务配置 */
TaskHandle_t display_task_handle;
#define DISPLAY_TASK_STACK 256
#define DISPLAY_TASK_PRIORITY 3
void App_Task_Display(void *pvParameters);

void App_Task_Init(void)
{            
        /* 创建消息队列 */
    sensorQueue = xQueueCreate(1, sizeof(SensorData_t));
    displayQueue = xQueueCreate(1, sizeof(DisplayData_t));
    bluetoothQueue = xQueueCreate(5, sizeof(int)); // 假设蓝牙数据为整数
    
    
    /* 1. 创建启动任务 */
    xTaskCreate(
        (TaskFunction_t)App_Task_Start,
        (char *)"App_Task_Start",
        (configSTACK_DEPTH_TYPE)START_TASK_STACK,
        (void *)NULL,
        (UBaseType_t)START_TASK_PRIORITY,
        (TaskHandle_t *)&start_task_handle);
            
    /* 2. 启动调度器 */
    vTaskStartScheduler();
}

/**
 * @description: 启动任务:用于创建其他任务
 * @param {void *} pvParameters
 * @return {*}
 */
void App_Task_Start(void *pvParameters)
{        BaseType_t xReturn = pdPASS;
    taskENTER_CRITICAL();
    xReturn=xTaskCreate(
        (TaskFunction_t)App_Task_GetData,
        (char *)"App_Task_GetData",
        (configSTACK_DEPTH_TYPE)DATA_TASK_STACK,
        (void *)NULL,
        (UBaseType_t)DATA_TASK_PRIORITY,
        (TaskHandle_t *)&data_task_handle);
            if(xReturn==pdPASS)
            printf("GetData TASK SUCCEED!\r\n");
            
    xReturn=xTaskCreate(
        (TaskFunction_t)App_Task_PID,
        (char *)"App_Task_PID",
        (configSTACK_DEPTH_TYPE)PID_TASK_STACK,
        (void *)NULL,
        (UBaseType_t)PID_TASK_PRIORITY,
        (TaskHandle_t *)&pid_task_handle);
            if(xReturn==pdPASS)
            printf("PID TASK SUCCEED!\r\n");
            
    xReturn=xTaskCreate(
        (TaskFunction_t)App_Task_Display,
        (char *)"App_Task_Display",
        (configSTACK_DEPTH_TYPE)DISPLAY_TASK_STACK,
        (void *)NULL,
        (UBaseType_t)DISPLAY_TASK_PRIORITY,
        (TaskHandle_t *)&display_task_handle);
            if(xReturn==pdPASS)
            printf("OLED TASK SUCCEED!\r\n");
            
    vTaskDelete(NULL);
    taskEXIT_CRITICAL();
}

/**
 * @description: 获取姿态数据:角度、编码器值
 * @param {void} *pvParameters
 * @return {*}
 */

void App_Task_GetData(void *pvParameters) {
    SensorData_t sensorData;
    float pitch, roll, yaw;
    short gyrox, gyroy, gyroz;

    while (1) {
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

        if (mpu_dmp_get_data(&pitch, &roll, &yaw) == 0) {
            MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);
        }
        int L_Speed = Read_Encoder(3);
        int R_Speed = Read_Encoder(4);

        /* 填充传感器数据结构 */
        sensorData.gyroz = gyroz;
                sensorData.gyrox = gyrox;
        sensorData.roll = roll;
        sensorData.L_Speed = L_Speed;
        sensorData.R_Speed = R_Speed;

        /* 发送到PID队列(覆盖旧数据) */
        xQueueOverwrite(sensorQueue, &sensorData);
    }
}

/**
 * @description: 进行PID控制的任务
 * @param {void} *pvParameters
 * @return {*}
 */
void App_Task_PID(void *pvParameters) {
    SensorData_t sensorData;
    DisplayData_t displayData;
        int Target,received_cmd;
        float Angle_turn = 0;
      
    while (1) {
        if (xQueueReceive(sensorQueue, &sensorData, portMAX_DELAY) == pdPASS) {
                    
                    /* 处理蓝牙指令队列 */
           while(xQueueReceive(bluetoothQueue, &received_cmd, 0) == pdPASS) {
                Bluetooth_data = received_cmd;  // 同步到全局变量
                            
                switch(Bluetooth_data) {
                    case 0x00:  // 刹车
                        Target = 0;
                        Angle_turn = 0;
                        break;
                    case 0x11:  // 前进
                        Target++;
                        break;
                    case 0x12:  // 后退
                        Target--;
                        break;
                    case 0x13:  // 左转
                        Angle_turn -= 5.0f;  // 每收到一次左转指令增加5度
                        break;
                    case 0x14:  // 右转
                        Angle_turn += 5.0f;
                        break;
                }
                Xianfu_Pwm(); // 限幅操作
            }
         

                /* PID计算 */
            pid.Pv = sensorData.roll * 10;
            stop_flag = Turn_Off(pid.Pv) ? 1 : 0;

            int Moto1 = balance(pid.Pv,sensorData.gyrox) + Velocity(Target, sensorData.L_Speed, sensorData.R_Speed) + turn(sensorData.gyroz, Angle_turn);
            int Moto2 = balance(pid.Pv,sensorData.gyrox) + Velocity(Target, sensorData.L_Speed, sensorData.R_Speed)- turn(sensorData.gyroz, Angle_turn);
            Xianfu_Pwm();
            Set_Pwm(Moto1, Moto2);

         

            /* 填充显示数据结构 */
//            displayData.pitch = sensorData.pitch;
            displayData.roll = sensorData.roll;
            displayData.L_Speed = sensorData.L_Speed;
            displayData.R_Speed = sensorData.R_Speed;
            displayData.Moto1 = Moto1;
            displayData.Moto2 = Moto2;
            displayData.Bluetooth_data = Bluetooth_data;

            /* 发送到显示队列 */
            xQueueOverwrite(displayQueue, &displayData);
        }
    }
}
/**
 * @description: OLED显示任务
 * @param {void} *pvParameters
 * @return {*}
 */
void App_Task_Display(void *pvParameters) {
    DisplayData_t displayData;

    while (1) {
        if (xQueueReceive(displayQueue, &displayData, portMAX_DELAY) == pdPASS) {
            OLED_ShowString(1, 1, "L:");
            OLED_ShowSignedNum(1, 3, displayData.L_Speed, 2);
            OLED_ShowString(1, 9, "R:");
            OLED_ShowSignedNum(1, 11, displayData.R_Speed, 2);
            OLED_ShowString(2, 1, "x:");
//            OLED_ShowSignedNum(2, 3, displayData.pitch, 2);
            OLED_ShowString(2, 9, "y:");
            OLED_ShowSignedNum(2, 11, displayData.roll, 2);
            OLED_ShowString(3, 1, "Blue:");
            OLED_ShowNum(3, 6, displayData.Bluetooth_data, 4);
        }
    }
}

void EXTI9_5_IRQHandler(void) {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    if (EXTI_GetITStatus(EXTI_Line8) == SET) {
//            printf("PWM" );
        // 发送通知前检查任务句柄有效性
        if (data_task_handle != NULL) {
            xTaskNotifyFromISR(data_task_handle, 0, eNoAction, &xHigherPriorityTaskWoken);
        }
        EXTI_ClearITPendingBit(EXTI_Line8);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}


//u8 Fore,Back,Left,Right;
void USART2_IRQHandler(void) {
       BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    int received_data = 0;  // 使用局部变量暂存接收数据
    
    if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) {
        received_data = USART_ReceiveData(USART2);
        Bluetooth_data = received_data;  // 更新全局变量
        xQueueSendFromISR(bluetoothQueue, &received_data, &xHigherPriorityTaskWoken);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}


  • 写回答

5条回答 默认 最新

  • 阿里嘎多学长 2025-03-14 18:43
    关注

    阿里嘎多学长整理AIGC生成,因移动端显示问题导致当前答案未能完全显示,请使用PC端查看更加详细的解答过程

    STM32及FreeRTOS的平衡小车

    你想将STM32的裸机程序改为FreeRTOS的三个任务,分别负责数据读取、PID控制和OLED显示。裸机程序在每次接收到6050的中断中执行,数据读取、PID输出PWM,进行控制。

    解决方案:

    1. 首先,需要安装FreeRTOS库,并将其集成到STM32项目中。
    2. 创建三个任务:数据读取任务、PID控制任务和OLED显示任务。
    3. 在数据读取任务中,使用中断处理函数来处理6050的中断,读取数据,并将数据传递给PID控制任务。
    4. 在PID控制任务中,使用PID算法来控制小车的速度和方向,并将控制信号传递给PWM输出任务。
    5. 在OLED显示任务中,使用OLED显示屏来显示小车的状态信息,例如速度、方向等。
    6. 使用FreeRTOS的调度机制来调度三个任务,确保任务之间的同步和协作。

    核心代码:

    #include "FreeRTOS.h"
    #include "task.h"
    #include "queue.h"
    
    // 数据读取任务
    void dataReadTask(void *pvParameter)
    {
        while (1)
        {
            // 读取6050的数据
            uint16_t data = read6050();
            // 将数据传递给PID控制任务
            xQueueSend(pidQueue, &data, portMAX_DELAY);
        }
    }
    
    // PID控制任务
    void pidTask(void *pvParameter)
    {
        while (1)
        {
            // 从队列中读取数据
            uint16_t data = xQueueReceive(pidQueue, NULL, portMAX_DELAY);
            // 使用PID算法来控制小车的速度和方向
            float pidOutput = pidAlgorithm(data);
            // 将控制信号传递给PWM输出任务
            xQueueSend(pwmQueue, &pidOutput, portMAX_DELAY);
        }
    }
    
    // OLED显示任务
    void oledTask(void *pvParameter)
    {
        while (1)
        {
            // 从队列中读取控制信号
            float pidOutput = xQueueReceive(pwmQueue, NULL, portMAX_DELAY);
            // 使用OLED显示屏来显示小车的状态信息
            oledDisplay(pidOutput);
        }
    }
    

    注意:上述代码只是一个示例,需要根据实际情况进行修改和优化。

    本回答被题主选为最佳回答 , 对您是否有帮助呢?
    评论
查看更多回答(4条)

报告相同问题?

问题事件

  • 系统已结题 8月10日
  • 已采纳回答 8月2日
  • 修改了问题 3月15日
  • 创建了问题 3月14日