将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);
}
}