注意：CCS Theia编译可能无法找到报错位置，建议每次小幅修改后就立即编译

主要问题：

  • 如何进行调参？

  • 如何设计一个完整的系统？

  • 小车如何走直线？

    • PID速度环
    • 陀螺仪

  • 小车在循迹时如何保证沿中线行驶？

  • 如何解决陀螺仪漂移的问题

    • 滤波算法

  • 如何避免/减少场地干扰？

  • 如何提高速度的同时保证行驶稳定？

程序结构-底层驱动

一、基础电机控制

源文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
#include "motor.h"
#include <iso646.h>

/**
  * @brief			电机启动
  * @param		    None
  * @retval 		None
  */
void Motor_On(void)
{
    DL_GPIO_setPins(GPIO_MOTOR_PIN_FSTBY_PORT, GPIO_MOTOR_PIN_FSTBY_PIN);//置位对应端口->引脚电平,STBY
}

/**
  * @brief		    电机关闭
  * @param		    None
  * @retval 		None		
  */
void Motor_Off(void)
{
    DL_GPIO_clearPins(GPIO_MOTOR_PIN_FSTBY_PORT, GPIO_MOTOR_PIN_FSTBY_PIN);//清除对应端口->引脚电平，STBY
    //逻辑口电平清除
    DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL1_PORT, GPIO_MOTOR_PIN_FL1_PIN);
    DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL2_PORT, GPIO_MOTOR_PIN_FL2_PIN);
    DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR1_PORT, GPIO_MOTOR_PIN_FR1_PIN);
    DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR2_PORT, GPIO_MOTOR_PIN_FR2_PIN);
}

/**
  * @brief			速度设置
  * @param		    左右轮占空比（百分值）
  * @retval 	    None
  */
void Set_Speed(float duty1,float duty2)
{
    uint32_t compareValue = 0;//比较值最大为3199，最小值为0
    if(duty1 < 0)
    {
        //PWM定时器向下计数，比较值计算
        compareValue = 3199 - 3199 * (-duty1/100.0);
        //比较值设置
        DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST, compareValue, DL_TIMER_CC_0_INDEX); 
        //电机逻辑引脚设置
        DL_GPIO_setPins(GPIO_MOTOR_PIN_FL2_PORT, GPIO_MOTOR_PIN_FL2_PIN);
        DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL1_PORT, GPIO_MOTOR_PIN_FL1_PIN);
       
    }
    else if(duty1 > 0)
    {
        compareValue = 3199 - 3199 * (duty1/100.0);
        DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST, compareValue, DL_TIMER_CC_0_INDEX);
        DL_GPIO_setPins(GPIO_MOTOR_PIN_FL1_PORT, GPIO_MOTOR_PIN_FL1_PIN);
        DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL2_PORT, GPIO_MOTOR_PIN_FL2_PIN);
    }
    else 
    {
        DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL1_PORT, GPIO_MOTOR_PIN_FL1_PIN);
        DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL2_PORT, GPIO_MOTOR_PIN_FL2_PIN);
    }

    if(duty2 < 0)
    {
        compareValue = 3199 - 3199 * (-duty2/100.0);
        DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST, compareValue, DL_TIMER_CC_1_INDEX);  
        DL_GPIO_setPins(GPIO_MOTOR_PIN_FR2_PORT, GPIO_MOTOR_PIN_FR2_PIN);
        DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR1_PORT, GPIO_MOTOR_PIN_FR1_PIN);
        

    }
    else if(duty2 > 0)
    {
        compareValue = 3199 - 3199 * (duty2/100.0);
        DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST, compareValue, DL_TIMER_CC_1_INDEX);
        DL_GPIO_setPins(GPIO_MOTOR_PIN_FR1_PORT, GPIO_MOTOR_PIN_FR1_PIN);
        DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR2_PORT, GPIO_MOTOR_PIN_FR2_PIN);
    }
    else 
    {
        DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR1_PORT, GPIO_MOTOR_PIN_FR1_PIN);
        DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR2_PORT, GPIO_MOTOR_PIN_FR2_PIN);
    }
}

头文件

1
2
3
4
5
6
7
8
9
10
11
#include "ti_msp_dl_config.h"

//宏定义简化写法
#define AIN1(x)   x?DL_GPIO_setPins(GPIO_MOTOR_PIN_FL1_PORT, GPIO_MOTOR_PIN_FL1_PIN):DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL1_PORT, GPIO_MOTOR_PIN_FL1_PIN)
#define AIN2(x)   x?DL_GPIO_setPins(GPIO_MOTOR_PIN_FL2_PORT, GPIO_MOTOR_PIN_FL2_PIN):DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL2_PORT, GPIO_MOTOR_PIN_FL2_PIN)
#define BIN1(x)   x?DL_GPIO_setPins(GPIO_MOTOR_PIN_FR1_PORT, GPIO_MOTOR_PIN_FR1_PIN):DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR1_PORT, GPIO_MOTOR_PIN_FR1_PIN)
#define BIN2(x)   x?DL_GPIO_setPins(GPIO_MOTOR_PIN_FR2_PORT, GPIO_MOTOR_PIN_FR2_PIN):DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR2_PORT, GPIO_MOTOR_PIN_FR2_PIN)

void Motor_On(void);
void Motor_Off(void);
void Set_Speed(float duty1,float duty2);

二、PID控制算法

# 参数定义

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
#include "PID.h"
#include "Serial.h"
#include "main.h"
#include "math.h"
#include "ti_msp_dl_config.h"

uint8_t Trace_Byte;//循迹总状态
uint8_t Angle_PID_Flag =0;//角度环开启标志位
uint8_t CV_flag=0;//视觉循迹开启标志位
uint8_t Test_pid_flag=0;//PID调试标志位
float K_trace = 0.0615;//减速系数
float Speed_midset = 30;//预设直线速度


/*灰度GPIO口宏定义*/
#define Read_Huidu_IO1	 ((DL_GPIO_readPins(GPIO_TRACE_PIN_TRACE_0_PORT,GPIO_TRACE_PIN_TRACE_0_PIN) == GPIO_TRACE_PIN_TRACE_0_PIN)?1:0)
#define Read_Huidu_IO2	 ((DL_GPIO_readPins(GPIO_TRACE_PIN_TRACE_1_PORT,GPIO_TRACE_PIN_TRACE_1_PIN) == GPIO_TRACE_PIN_TRACE_1_PIN)?1:0)
#define Read_Huidu_IO3	 ((DL_GPIO_readPins(GPIO_TRACE_PIN_TRACE_2_PORT,GPIO_TRACE_PIN_TRACE_2_PIN) == GPIO_TRACE_PIN_TRACE_2_PIN)?1:0)
#define Read_Huidu_IO4	 ((DL_GPIO_readPins(GPIO_TRACE_PIN_TRACE_3_PORT,GPIO_TRACE_PIN_TRACE_3_PIN) == GPIO_TRACE_PIN_TRACE_3_PIN)?1:0)
#define Read_Huidu_IO5	 ((DL_GPIO_readPins(GPIO_TRACE_PIN_TRACE_4_PORT,GPIO_TRACE_PIN_TRACE_4_PIN) == GPIO_TRACE_PIN_TRACE_4_PIN)?1:0)
#define Read_Huidu_IO6	 ((DL_GPIO_readPins(GPIO_TRACE_PIN_TRACE_5_PORT,GPIO_TRACE_PIN_TRACE_5_PIN) == GPIO_TRACE_PIN_TRACE_5_PIN)?1:0)
#define Read_Huidu_IO7	 ((DL_GPIO_readPins(GPIO_TRACE_PIN_TRACE_6_PORT,GPIO_TRACE_PIN_TRACE_6_PIN) == GPIO_TRACE_PIN_TRACE_6_PIN)?1:0)
#define Read_Huidu_IO8	 ((DL_GPIO_readPins(GPIO_TRACE_PIN_TRACE_7_PORT,GPIO_TRACE_PIN_TRACE_7_PIN) == GPIO_TRACE_PIN_TRACE_7_PIN)?1:0)

结构体类型

创建结构体类型的方法，方便进行同类型变量元素组的创建，减少代码重复度。

这里定义了PID类型结构体，在之后新建其他用于PID控制的变量组时，只要用PID类型定义变量即可、

1
2
3
4
5
6
7
8
9
10
11
12
/*结构体类型定义*/
typedef struct 
{
	float Kp;
	float Ki;
	float Kd;
	float error;
	float last_error;
	float error_sum;
	float error_difference;
	float velocity_sum;
}PID;

PID初始化相关函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
/*PID类型变量声明*/
PID Velocity;
PID Velocity_L;
PID trace_hd;

/*速度环PID*/

void Velocity_PID_Init()
{
    Velocity.Kp = -1.33;
    Velocity.Kd = -0.22;
    Velocity.Ki = -0.012;
}

void Velocity_L_PID_Init()
{
    Velocity_L.Kp = -1.34;
    Velocity_L.Kd = -0.22;
    Velocity_L.Ki = -0.012;
}

/*循迹环PID*/
void trace_hd_PID_Init()
{
    trace_hd.Kp = -1.75;
    trace_hd.Kd = -0.65;
    trace_hd.Ki = 0;
}

/*PID初始化*/
void PID_Init()
{
    Velocity_PID_Init();
    Velocity_L_PID_Init();
    trace_hd_PID_Init();
}

/*限幅函数*/
void I_amplitude_limiting(float number,float *Error_sum)
{
	if(*Error_sum > number)
	{
		*Error_sum = number;
	}
	
	if(*Error_sum <- number)
	{
		*Error_sum = -number;
	}
}

辅助函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
/*限幅函数*/
void I_amplitude_limiting(float number,float *Error_sum)
{
	if(*Error_sum > number)
	{
		*Error_sum = number;
	}
	
	if(*Error_sum <- number)
	{
		*Error_sum = -number;
	}
}

/**
  * @brief			获取灰度巡线路数
  * @param		    无
  * @retval 		识别路数			
  */
char Huidu_Counter()
{
    uint8_t hd_sum = 0;
    
	for(int i=0;i<7;i++)//灰度循迹个数统计
	{
		if(((~Trace_Byte)>>i)&0x01) hd_sum++;
	}

    return hd_sum;
}

/**
  * @brief			灰度状态读取函数
  * @param		    无
  * @retval 		无			
  */
void Get_TraceData()
{
    Trace_Byte = (Read_Huidu_IO1<<7) + (Read_Huidu_IO2 << 6) + (Read_Huidu_IO3 << 5) + (Read_Huidu_IO4 <<4) 
                 + (Read_Huidu_IO5 << 3) +(Read_Huidu_IO6 <<2) +(Read_Huidu_IO7 <<1) + Read_Huidu_IO8;//8路灰度循迹状态
}

1.循迹控制（PD控制+自适应曲线减速）

此函数包含：

  • 循迹路数统计：可用于判断小车是否脱线、循线、特征值检测
  • 适用于圆弧的循迹状态穷举
  • 自适应的曲线减速函数，提高循迹的稳定性
  • PD环控制，提高循迹的抗干扰能力与拟合程度
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
/*八路循迹环PID控制器*/
float Turn_hd_PID()
{	
    uint8_t temp_hd_sum = 0;
    
    temp_hd_sum = Huidu_Counter();

    if(temp_hd_sum <=2)//差速循迹
    {
        switch(Trace_Byte)
        {
            case 0xE7: // 1110 0111
            case 0xC3: // 1100 0011
                trace_hd.error = 0;
                break;
            case 0x00: // 0000 0000
            case 0x0F: // 0000 1111
            case 0x07: // 0000 0111
                trace_hd.error = -9;
                break;
            case 0xF0: // 1111 0000
            case 0xE0: // 1110 0000
                trace_hd.error = 9;
                break;
            case 0xE3: // 1110 0011
            case 0xF7: // 1111 0111
                trace_hd.error = 2;
                break;
            case 0xC1: // 1100 0001
                trace_hd.error = 1;
                break;
            case 0xF3: // 1111 0011
                trace_hd.error = 4;
                break;
            case 0xF1: // 1111 0001
            case 0xFB: // 1111 1011
                trace_hd.error = 6;
                break;
            case 0xF9: // 1111 1001
            case 0xFD: // 1111 1101
                trace_hd.error = 8;
                break;
            case 0xF8: // 1111 1000
                trace_hd.error = 10;
                break;
            case 0xFC: // 1111 1100
                trace_hd.error = 12;
                break;
            case 0xFE: // 1111 1110
                trace_hd.error = 14;
                break;
            case 0x87: // 1000 0111
                trace_hd.error = -2;
                break;
            case 0xC7: // 1100 0111
                trace_hd.error = -1;
                break;
            case 0xEF: // 1110 1111
                trace_hd.error = -2;
                break;
            case 0xCF: // 1100 1111
                trace_hd.error = -4;
                break;
            case 0x8F: // 1000 1111
            case 0xDF: // 1101 1111
                trace_hd.error = -6;
                break;
            case 0x9F: // 1001 1111
            case 0xBF: // 1011 1111
                trace_hd.error = -8;
                break;
            case 0x1F: // 0001 1111
                trace_hd.error = -10;
                break;
            case 0x3F: // 0011 1111
                trace_hd.error = -12;
                break;
            case 0x7F: // 0111 1111
                trace_hd.error = -14;
                break;
            case 0xFF: // 1111 1111
                trace_hd.error = trace_hd.last_error > 0 ? 16 : -16;
                break;
            default:
                trace_hd.error = 0;
                break;
        }        
    }
    else if(temp_hd_sum >= 6 && temp_hd_sum <= 8)//掉头
    {
        //Angle_PID_Flag = 1;
    }
    else//停车
    {

    }

    trace_hd.error_difference = trace_hd.error - trace_hd.last_error;
    trace_hd.error_sum += trace_hd.error;//误差累加量
    trace_hd.last_error = trace_hd.error;
    I_amplitude_limiting(1000,&trace_hd.error_sum);//误差累加量限幅


    K_trace =  1/16.0 * pow((1-(20/Speed_midset)),0.5);//弯道减速系数
    MID_Speed = Speed_midset * (1 - (uint8_t)(trace_hd.error)*K_trace) * (1 + (uint8_t)(trace_hd.error)*K_trace);//基准速度变换

    return trace_hd.error*trace_hd.Kp  + trace_hd.error_difference * trace_hd.Kd;//PD环循迹，比例+积分
}

2.陀螺仪角度环(小车姿态控制)

此函数功能：

  • 输入陀螺仪传感器测量的姿态，进行PID解算
  • PD环控制，确保小车转向迅速并且稳定。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
/*转向环PID控制器*/
float Turn_imu_PID(int yaw, int caclu_yaw)
{	
    float Kp = -1.35;
    float Kd = -9.5;
    float Ki = 0;

    static float error ;
    static float last_error;
    static float error_diff;
    static float error_sum;

    error = yaw -caclu_yaw;
    error_sum += error;

    I_amplitude_limiting(1000,&error_sum);//误差累加量限幅

    error_diff = error-last_error;
    last_error = error;

    
    if (error > 180) // 防止小车转到180度时一直旋转的问题
        error = error - 360;
    if (error < -180)
        error = error + 360;
    

    return Kp * error + Kd * error_diff + Ki*error_sum;
}

PID头文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
#include"ti_msp_dl_config.h"
extern uint8_t Trace_Byte;
extern uint8_t Angle_PID_Flag;
extern uint8_t Test_pid_flag;
extern float Speed_midset;//预设直线速度

float Turn_cv_PID(int measure, int caclu);
float Turn_hd_PID();
float Turn_hd_PID_Seven();
float Velocity_PID_L(float velocity,float velocity_calcu);
float Velocity_PID_R(float velocity,float velocity_calcu);
void PID_Init();
void Get_TraceData();
float Turn_imu_PID(int yaw, int caclu_yaw);
char Huidu_Counter();

三、陀螺仪串口通信

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
#include "ti_msp_dl_config.h"
#include "Delay.h"

// 定义接收变量
uint8_t RollL, RollH, PitchL, PitchH, YawL, YawH, VL, VH, SUM;
float Pitch,Roll,Yaw;
// 串口接收状态标识
#define WAIT_HEADER1 0
#define WAIT_HEADER2 1
#define RECEIVE_DATA 2

uint8_t RxState_JY = WAIT_HEADER1;
uint8_t receivedData[9];
uint8_t dataIndex = 0;

//发送置偏航角置零命令
void Serial_JY61P_Zero_Yaw(void){
    DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XFF);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XAA);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X69);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X88);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XB5);
	delay_ms(100);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XFF);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XAA);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X01);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X04);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
	delay_ms(100);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XFF);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XAA);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
	
}

头文件

1
2
3
4
5
6
7
#ifndef __JY61P_H
#define __JY61P_H

void Serial_JY61P_Zero_Yaw(void);
void UART_JY61P_INST_IRQHandler(void);
extern float Pitch,Roll,Yaw;
#endif

四、常规外设串口通信

1. 变量声明

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
#include "stdio.h"
#include "string.h"
#include "ti_msp_dl_config.h"
#include "main.h"
#define BUFFER_SIZE 256  // 定义缓冲区大小

uint8_t Serial_RxPacket[100];
/*PID调试*/
float Test_Kp;//比例
float Test_Ki;//积分
float Test_Kd;//微分
float Test_Ks;//目标速度

/*系统调试串口变量*/
uint8_t Serial_RxFlag;
uint8_t RxState = 0;
uint8_t pRxState;//表示当前接收的是第几个变量
uint8_t RxData_type;

/*上位机串口变量*/
uint8_t Soc_RxFlag;
uint8_t RxState_SOC = 0;
uint8_t SOC_RxData_type;
uint8_t SOC_pRxState;//串口接收数据索引号

/*串口屏串口变量*/
uint8_t Screen_RxFlag;
uint8_t RxState_Screen;
uint8_t Screen_RxData_type;
uint8_t Screen_pRxState;//串口接收数据索引号

uint8_t Serial_RxPacket[100];

2. 辅助函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
//系统串口初始化
void SYS_UART0_Init(void)
{
	NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN);
	NVIC_EnableIRQ(UART_0_INST_INT_IRQN);
    DL_UART_clearInterruptStatus(UART_0_INST,DL_UART_INTERRUPT_RX);//清除中断标志位
}

//SOC通信串口初始化
void SOC_UART1_Init(void)
{
	NVIC_ClearPendingIRQ(UART_SOC_INST_INT_IRQN);
	NVIC_EnableIRQ(UART_SOC_INST_INT_IRQN);
    DL_UART_clearInterruptStatus(UART_SOC_INST,DL_UART_INTERRUPT_RX);//清除中断标志位
}

//发送字符串给SOC
void SOC_SendString(char *str)
{
    while(*str != '\0')
    {
        DL_UART_Main_transmitDataBlocking(UART_SOC_INST, *str++);
    }
}

//串口屏通信串口初始化
void Screen_UART2_Init(void)
{
	NVIC_ClearPendingIRQ(UART_Screen_INST_INT_IRQN);
	NVIC_EnableIRQ(UART_Screen_INST_INT_IRQN);
    DL_UART_clearInterruptStatus(UART_Screen_INST,DL_UART_INTERRUPT_RX);//清除中断标志位
}

//发送字符串给串口屏
void Screen_SendString(char *str)
{
    while(*str != '\0')
    {
        DL_UART_Main_transmitDataBlocking(UART_Screen_INST, *str++);
    }
}

void int_to_binary_string(uint32_t value, char *binary_str, int max_bits)
{
    // 生成二进制字符串
    for (int i = max_bits - 1; i >= 0; --i)
    {
        binary_str[i] = (value & (1U << i)) ? '1' : '0';
    }
    binary_str[max_bits] = '\0'; // 确保字符串以 null 结尾
}

// 将浮点数转换为字符串
void float_to_string(float value, char *str, size_t size)
{
    snprintf(str, size, "%.2f", value); // 格式化为两位小数
}

void HMI_send_string(char *name, char *showdata)
{
    char buffer[BUFFER_SIZE];
    int length;

    // 构造要发送的字符串，确保缓冲区足够大
    length = snprintf(buffer, sizeof(buffer), "%s=\"%s\"\xff\xff\xff", name, showdata);

    // 检查是否发生了缓冲区溢出
    if (length >= sizeof(buffer))
    {
        // 缓冲区溢出处理，例如可以通过截断字符串或者增加缓冲区大小来解决
        // 这里只是简单地截断字符串以确保不会发送超出缓冲区的内容
        buffer[sizeof(buffer) - 1] = '\0'; // 确保字符串以 null 结尾
    }

    // 使用 Screen_SendString 发送构造好的字符串
    Screen_SendString(buffer);
}

void HMI_send_number(char* name, int num)
{
    char buffer[BUFFER_SIZE];
    int length = snprintf(buffer, sizeof(buffer), "%s=%d\xff\xff\xff", name, num);
    if (length >= BUFFER_SIZE)
    {
        // 缓冲区溢出处理
    }
    Screen_SendString(buffer);
}

void HMI_send_float(char* name, float num)
{
    char buffer[BUFFER_SIZE];
    int num_int = (int)(num * 100);
    int length = snprintf(buffer, sizeof(buffer), "%s=%d\xff\xff\xff", name, num_int);
    if (length >= BUFFER_SIZE)
    {
        // 缓冲区溢出处理
    }
    Screen_SendString(buffer);
}

void HMI_Wave(char* name, int ch, int val)
{
    char buffer[BUFFER_SIZE];
    int length = snprintf(buffer, sizeof(buffer), "add %s,%d,%d\xff\xff\xff", name, ch, val);
    if (length >= BUFFER_SIZE)
    {
        // 缓冲区溢出处理
    }
    Screen_SendString(buffer);
}

void HMI_Wave_Fast(char* name, int ch, int count, int* show_data)
{
    char buffer[BUFFER_SIZE];
    int length = snprintf(buffer, sizeof(buffer), "addt %s,%d,%d\xff\xff\xff", name, ch, count);
    if (length >= BUFFER_SIZE)
    {
        // 缓冲区溢出处理
    }
    Screen_SendString(buffer);

    delay_ms(100);

    for (int i = 0; i < count; i++)
    {
        // 发送每个字符
        DL_UART_Main_transmitDataBlocking(UART_Screen_INST, (char)show_data[i]);
    }
    // 发送结束标志
    Screen_SendString("\xff\xff\xff");
}

void HMI_Wave_Clear(char* name, int ch)
{
    char buffer[BUFFER_SIZE];
    int length = snprintf(buffer, sizeof(buffer), "cle %s,%d\xff\xff\xff", name, ch);
    if (length >= BUFFER_SIZE)
    {
        // 缓冲区溢出处理
    }
    Screen_SendString(buffer);
}

程序结构-中断服务函数

1.常规串口中断

  • Uart0: 系统串口，用作与计算机通信调试
  • Uart_Screen：串口屏串口，用于和串口屏进行通信调试，串口屏可以直接控制小车，也可以显示小车实时参数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
void UART_0_INST_IRQHandler(void)
{
    switch (DL_UART_Main_getPendingInterrupt(UART_0_INST)) {//判断中断的类型，DL_UART_Main_getPendingInterrupt(UART_0_INST)调用具有清空标志位的功能？
        case DL_UART_MAIN_IIDX_RX://如果触发串口接收事件
       RxData = DL_UART_Main_receiveData(UART_0_INST);
		if(RxState == 0)//帧头检测
		{
			if(RxData == '#' && Serial_RxFlag == 0)
			{
				RxState=1;
			}
		}
		else if(RxState == 1)//数据类型检测s
		{
			switch(RxData)
			{
				case 'P'://x+
					RxState=2;
					RxData_type=1;
					break;
				case 'I'://x-
					RxState=2;
					RxData_type=2;
					break;
				case 'D'://x-
					RxState=2;
					RxData_type=3;
					break;
				case 'S':
					RxState=2;
					RxData_type=4;
					break;					
			}
		}
		else if(RxState==2)//数据，接收范围限制在单字节，足够完成任务要求
		{
			if(RxData == '!')
			{
				Serial_RxFlag=1;
				RxState=0;			
			}
			else
			{
				Serial_RxPacket[pRxState]=RxData;
				pRxState++;		
			}
		}
            break;
        default:
            break;
    }
}

void UART_Screen_INST_IRQHandler(void)
{
    switch (DL_UART_Main_getPendingInterrupt(UART_Screen_INST))
    {
        case DL_UART_MAIN_IIDX_RX://如果触发串口接收事件
            RxData = DL_UART_Main_receiveData(UART_Screen_INST);
            if(RxState_Screen == 0)//帧头检测
            {
                if(RxData == 0xEF && Screen_RxFlag == 0)
                {
                    RxState_Screen = 1;//串口屏的数据帧
                }
            }
            else if(RxState_Screen == 1)
            {
                Serial_RxPacket[0]=RxData;
                RxState_Screen = 2;
            }
            else if(RxState_Screen == 2)
            {
                if(RxData == 0xEE)
                {
                    Screen_RxFlag=1;
                    RxState_Screen=0;
                }
            }
            break;
        default:
            break;
    }
}

2. 陀螺仪串口中断函数

用于处理从陀螺仪传感器接收到的通信数据，其中主要的部分是Roll，Pitch，Yaw角

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
// 串口中断处理函数
void UART_JY61P_INST_IRQHandler(void) {
    uint8_t uartdata = DL_UART_Main_receiveData(UART_JY61P_INST); // 接收一个uint8_t数据

    switch (RxState_JY) {
    case WAIT_HEADER1:
        if (uartdata == 0x55) {
            RxState_JY = WAIT_HEADER2;
        }
        break;
    case WAIT_HEADER2:
        if (uartdata == 0x53) {
            RxState_JY = RECEIVE_DATA;
            dataIndex = 0;
        } else {
            RxState_JY = WAIT_HEADER1; // 如果不是期望的第二个头，重置状态
        }
        break;
    case RECEIVE_DATA:
        receivedData[dataIndex++] = uartdata;
        if (dataIndex == 9) {
            // 数据接收完毕，分配给具体的变量
            RollL = receivedData[0];
            RollH = receivedData[1];
            PitchL = receivedData[2];
            PitchH = receivedData[3];
            YawL = receivedData[4];
            YawH = receivedData[5];
            VL = receivedData[6];
            VH = receivedData[7];
            SUM = receivedData[8];

            // 校验SUM是否正确
            uint8_t calculatedSum = 0x55 + 0x53 + RollH + RollL + PitchH + PitchL + YawH + YawL + VH + VL;
            if (calculatedSum == SUM) {
                // 校验成功，可以进行后续处理
                if((float)(((uint16_t)RollH << 8) | RollL)/32768*180>180){
                    Roll = (float)(((uint16_t)RollH << 8) | RollL)/32768*180 - 360;
                }else{
                    Roll = (float)(((uint16_t)RollH << 8) | RollL)/32768*180;
                }

                if((float)(((uint16_t)PitchH << 8) | PitchL)/32768*180>180){
                    Pitch = (float)(((uint16_t)PitchH << 8) | PitchL)/32768*180 - 360;
                }else{
                    Pitch = (float)(((uint16_t)PitchH << 8) | PitchL)/32768*180;
                }

                if((float)(((uint16_t)YawH << 8) | YawL)/32768*180 >180){
                    Yaw = (float)(((uint16_t)YawH << 8) | YawL)/32768*180 - 360;
                }else{
                    Yaw = (float)(((uint16_t)YawH << 8) | YawL)/32768*180;
                }
                
                
            } else {
                // 校验失败，处理错误
            }

            RxState_JY = WAIT_HEADER1; // 重置状态以等待下一个数据包
        }
        break;
    }

//DL_UART_Main_transmitData(UART_JY61P_INST, uartdata); // 可选：回传接收到的数据
}

程序结构-任务进程

在此文件中，定义各种不同的任务

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
#include "Process.h"

/**************************串口进程*********************************/
/*系统串口(UART1)解码进程*/
void SYS_RxPro()
{
    /*PID调试*/
    if(Serial_RxFlag)
    {
        switch(RxData_type)//判断数据帧的数据类型
        {
            case 1:
                if(pRxState == 1)
                {
                    Test_Kp = Serial_RxPacket[0]-0x30;
                }
                else if(pRxState == 2)
                {
                    Test_Kp = (Serial_RxPacket[0]-0x30)*10 + (Serial_RxPacket[1]-0x30);
                }
                else if(pRxState == 3)
                {
                    Test_Kp = (Serial_RxPacket[0]-0x30)*100 + (Serial_RxPacket[1]-0x30)*10 + (Serial_RxPacket[2]-0x30);
                }
                break;
            case 2:
                if(pRxState == 1)
                {
                    Test_Ki = Serial_RxPacket[0]-0x30;
                }
                else if(pRxState == 2)
                {
                    Test_Ki = (Serial_RxPacket[0]-0x30)*10 + (Serial_RxPacket[1]-0x30);
                }
                else if(pRxState == 3)
                {
                    Test_Ki = (Serial_RxPacket[0]-0x30)*100 + (Serial_RxPacket[1]-0x30)*10 + (Serial_RxPacket[2]-0x30);
                }				
                break;
            case 3:
                if(pRxState == 1)
                {
                    Test_Kd = Serial_RxPacket[0]-0x30;
                }
                else if(pRxState == 2)
                {
                    Test_Kd = (Serial_RxPacket[0]-0x30)*10 + (Serial_RxPacket[1]-0x30);
                }
                else if(pRxState == 3)
                {

                    Test_Kd = (Serial_RxPacket[0]-0x30)*100 + (Serial_RxPacket[1]-0x30)*10 + (Serial_RxPacket[2]-0x30);
                }						
                break;
            case 4:
                if(pRxState == 1)
                {
                    Test_Ks = Serial_RxPacket[0]-0x30;
                }
                else if(pRxState == 2)
                {
                    Test_Ks = (Serial_RxPacket[0]-0x30)*10 + (Serial_RxPacket[1]-0x30);
                }
                else if(pRxState == 3)
                {
                    Test_Ks = (Serial_RxPacket[0]-0x30)*100 + (Serial_RxPacket[1]-0x30)*10 + (Serial_RxPacket[2]-0x30);
                }						
                break;
        }
        Serial_RxFlag=0;	
        pRxState=0;
    }
}

/*串口屏通信解码进程*/
void Screen_RxPro()
{
    if(Screen_RxFlag == 1)
    {
        switch(Serial_RxPacket[0])
        {
            case 1:
                Motor_On();
                Angle_PID_Flag = 1;

                if(System_Mode == 0 | System_Mode == 1)
                {
                    yaw_detect = yaw_val;
                }
                else
                {
                    yaw_detect = yaw_val - LX_3_IMU_ANGEL_1;
                }

                Motor_flag = 1;
                break;
            case 2:
                Serial_JY61P_Zero_Yaw();
                Motor_Off();
                break;
            case 3://PID参数模式切换：调试模式<->固定模式
                Test_pid_flag=1;
            case 4:
                Test_pid_flag=0;
                break;
        }
        DL_UART_Main_transmitData(UART_Screen_INST,Serial_RxPacket[0]);
        Screen_RxFlag=0;
    }
}

/********************特殊功能进程**********************/
void Beep_Proc()
{
    if(beep_flag == 1 || beep_key_flag == 1)
    {
        DL_GPIO_setPins(GPIO_BEEP_PORT,GPIO_BEEP_PIN_1_PIN);
    }
    else 
    {
        DL_GPIO_clearPins(GPIO_BEEP_PORT,GPIO_BEEP_PIN_1_PIN);
    }
}

/*OlED进程*/
void Oled_Proc()
{
    OLED_ShowString(0, 8,"Mode:",8);
    OLED_ShowNum(42, 8, System_Mode+1, 1 ,8);
    OLED_ShowBinNum(0, 28, Trace_Byte, 8 ,8);
    OLED_ShowSignedNum(0, 45,Yaw,3,8);
    OLED_Update();    
}

/*串口屏进程*/
void HMI_Proc()
{
    int_to_binary_string(Trace_Byte, data_HMI, 8);
    HMI_send_string(hmi_trace, data_HMI);//串口屏打印循迹状态

    float_to_string(Yaw,data_HMI,8);
    HMI_send_string(hmi_yaw, data_HMI);//串口屏打印陀螺仪实际的Yaw角度

    float_to_string(M_Speed_L,data_HMI,8);
    HMI_send_string(hmi_speed, data_HMI);//串口屏打印速度

    float_to_string(System_Mode,data_HMI,8);
    HMI_send_string(hmi_imu, data_HMI);//串口屏打印单片机记忆的Yaw角度

    float_to_string(NEncoder.right_motor_total_cnt,data_HMI,8);
    HMI_send_string(hmi_totol, data_HMI);//串口屏打印总的脉冲数
}

/*按键*/
void Key_Proc(void)//按键检测程序
{
	if(Key_Timer) return;//Key_Timer=0时执行下面的语句
	Key_Timer=1;

	//按键扫描部分//
	Key_Val=Key_Read();
	Key_Down=Key_Val & (Key_Old ^ Key_Val);
	Key_Up=~Key_Val & (Key_Old ^ Key_Val);
	Key_Old=Key_Val;

    switch(Key_Down)
    {
        case 1:
            beep_key_flag = 1;
            if(++System_Mode == 4)
            {
                System_Mode = 0;
            }
            break;
        case 2:
            Motor_On();
            Angle_PID_Flag = 1;
            Speed_midset = Speed_ZX;
            if(System_Mode == 0 | System_Mode == 1)
            {
                yaw_detect = yaw_val;
            }
            else 
            {
                yaw_detect = yaw_val - LX_3_IMU_ANGEL_1;
            }

            Motor_flag = 1;
            break;
        case 3:
            Motor_flag = 0;
            Serial_JY61P_Zero_Yaw();
            Motor_Off();            
        break;
    }
}

程序结构-路径规划

1. 路径1

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
/*******************************路线进程***********************************/
//要求1
void LX_Proc_1()
{
    uint8_t temp_hd_sum;
    static uint8_t LX_state = 0;

    temp_hd_sum =  Huidu_Counter();

    if(LX_state ==0)
    {
        Speed_midset = 30 - NEncoder.right_motor_total_cnt/8000 * 15.0;

        if(temp_hd_sum >=1)
        {
            beep_flag = 1;
            LX_state = 1;
            Motor_Off();
        }
    }
}

2. 路径2

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
//要求2
void LX_Proc_2()
{
    uint8_t temp_hd_sum;
    static uint8_t LX_state = 0;
    static uint32_t LuChen_Counter;
    
    temp_hd_sum =  Huidu_Counter();

    //LuChen_Counter = (NEncoder.left_motor_total_cnt + NEncoder.right_motor_total_cnt)/2;

    if(LX_state ==0)//从A触发
    {
        Speed_midset = 40;
        if(temp_hd_sum >=1)//碰到B
        {
            Speed_midset = 35;
            LX_state = 1;
            Angle_PID_Flag = 0;
            beep_flag = 1;
            motor_total_cnt_reset();
        }
    }
    else if(LX_state == 1)//从B出发
    {
        if(NEncoder.right_motor_total_cnt >= 5000)
        {
            if(temp_hd_sum == 0)//碰到C
            {
                Speed_midset = 40;
                yaw_detect -= LX_2_IMU_ANGEL;
                Angle_PID_Flag = 1;
                LX_state = 2;
                beep_flag = 1;
            }                   
        }
    }
    else if(LX_state == 2)//从C出发
    {
        if(temp_hd_sum >= 1)//碰到D
        {
            motor_total_cnt_reset();
            Speed_midset = 35;
            Angle_PID_Flag = 0;
            LX_state = 3;
            beep_flag = 1;     
        }    
    }
    else if(LX_state == 3)//从D出发
    {
        if(NEncoder.right_motor_total_cnt >= 5000)
        {
            if(temp_hd_sum == 0)//碰到A
            {
                LX_state = 4;
                beep_flag = 1;
                Motor_Off();
            }                 
        }
                   
    }
}

3. 路径3

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
//要求3
void LX_Proc_3()
{
    uint8_t temp_hd_sum;
    static uint8_t LX_state = 0;
    static uint8_t i;


    temp_hd_sum =  Huidu_Counter();

    if(LX_state ==0)//从A触发
    {
        Speed_midset=0;
        if(Angle_PID_Flag)
        {
            Timer_Angel_Sleep_flag = 1;

            if(fabsf(Yaw - yaw_detect) <= 1.5)
            {
                Speed_midset=Speed_ZX-15;
                beep_flag = 1;
                LX_state = 11;
            }            
        }
    }
    else if(LX_state == 11)
    {

        if(temp_hd_sum >=1)//碰到C
        {
            Speed_midset=Speed_WD-10;
            LX_state = 1;
            Angle_PID_Flag = 0;
            beep_flag = 1;
            motor_total_cnt_reset();
        }        
    }
    else if(LX_state == 1)//从C出发
    {
        if(NEncoder.right_motor_total_cnt > 5000)
        {
            if(temp_hd_sum == 0)//碰到B
            {
                Speed_midset=Speed_ZX-15;
                yaw_detect = yaw_val + LX_4_IMU_ANGEL_1 + 0.5;
                Angle_PID_Flag = 1;
                LX_state = 2;
                beep_flag = 1;
            }                    
        }
    }
    else if(LX_state == 2)//从B出发
    {
        if(temp_hd_sum >= 1)//碰到D
        {
            Speed_midset=Speed_WD-10;
            Angle_PID_Flag = 0;
            LX_state = 3;
            beep_flag = 1;     
            motor_total_cnt_reset();
        }    
    }
    else if(LX_state == 3)//从D出发
    {
        if(NEncoder.right_motor_total_cnt > 5000)
        {
            if(temp_hd_sum == 0)//碰到A
            {
                beep_flag = 1;
                LX_state = 4;
                Motor_Off();
            }              
        }             
    }
}

4. 路径4

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
//要求4
void LX_Proc_4()
{
    uint8_t temp_hd_sum;
    static uint8_t LX_state = 0;
    static uint8_t i;

    temp_hd_sum =  Huidu_Counter();
    K_ZX = 1-(20/Speed_ZX);

    if(LX_state ==0)//从A触发
    {
        Speed_midset=0;

        if(Angle_PID_Flag)
        {
            Timer_Angel_Sleep_flag = 1;
            if(fabsf(Yaw - yaw_detect) <= 1.5)
            {
                motor_total_cnt_reset();

                Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX * K_ZX);

                beep_flag = 1;
                LX_state = 11;
            }            
        }
    }
    else if(LX_state == 11)
    {
        Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX *K_ZX);
        if(NEncoder.right_motor_total_cnt > 4000)
        {
            if(temp_hd_sum >=1)//碰到C
            {
                Speed_midset=Speed_WD;
                LX_state = 1;
                Angle_PID_Flag = 0;
                beep_flag = 1;
                motor_total_cnt_reset();
            }    
        }
     
    }
    else if(LX_state == 1)//从C出发
    {
        if(NEncoder.right_motor_total_cnt > 5000)
        {
            if(temp_hd_sum == 0)//碰到B
            {
                motor_total_cnt_reset();
                Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX * K_ZX);
                yaw_detect = yaw_val + LX_4_IMU_ANGEL_1 + 0.65;
                Angle_PID_Flag = 1;
                LX_state = 2;
                beep_flag = 1;
            }                    
        }
    }
    else if(LX_state == 2)//从B出发
    {
        Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX * K_ZX);
        if(NEncoder.right_motor_total_cnt > 4000)
        {
            if(temp_hd_sum >= 1)//碰到D
            {
                Speed_midset=Speed_WD;
                Angle_PID_Flag = 0;
                LX_state = 3;
                beep_flag = 1;     
                motor_total_cnt_reset();

                if(i == 3)
                {
                    Speed_midset = Speed_WD - 5;
                }
            }              
        }
    }
    else if(LX_state == 3)//从D出发
    {
        if(NEncoder.right_motor_total_cnt > 5000)
        {
            if(temp_hd_sum == 0)//碰到A
            {
                motor_total_cnt_reset();
                Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX * K_ZX);
                LX_state = 12;
                Angle_PID_Flag = 1;
                yaw_detect = yaw_val - LX_4_IMU_ANGEL_1 + 1.0;                
                beep_flag = 1;
                if(++i>=4)
                {
                    LX_state = 4;
                    Motor_Off();
                }
            }              
        }             
    }
    else if(LX_state == 12)//从A出发
    {
        Speed_midset = Speed_ZX - (NEncoder.right_motor_total_cnt/12000 * Speed_ZX * K_ZX);

        if(NEncoder.right_motor_total_cnt > 4000)
        {
            if(temp_hd_sum >= 1)//碰到C
            {
                Speed_midset=Speed_WD;
                Angle_PID_Flag = 0;
                LX_state = 1;
                beep_flag = 1;     
                motor_total_cnt_reset();
            }       
        }
    }
}

程序结构-主程序

1. 参数声明

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
#include "main.h"
#include "OLED.h"
#include "Serial.h"
#include "nqei.h"
#include "ti/driverlib/dl_gpio.h"
#include "ti_msp_dl_config.h"
#include <stdlib.h>
#include "Path.c"

uint32_t Key_Number;//按键调试 
uint8_t Key_Val,Key_Down,Key_Up,Key_Old;

//PS:PWM可调范围，0~100以内任意浮点数
float MT_L = 20.0;//左轮速度初值
float MT_R = 20.0;//右轮速度初值

uint8_t RxData;//串口接收寄存器存储变量
uint8_t MID_Speed;//基准速度

float yaw_val;//记录Yaw角
float yaw_detect;//预测yaw角

static float Velocity_IL,Velocity_IR;//左右轮速度环PID输入
static float Dif_Out;//差速环PID输出

uint8_t Motor_flag;//电机使能标志位

float M_Speed_L;//左轮测速
float M_Speed_R;//右轮测速

uint32_t Timer_Angel_Sleep;
uint8_t Timer_Angel_Sleep_flag;

//外设列表：
//1.GPIO        LED
//2.PWM         定时器G0,两路PWM
//3.定时器      定时器A0
//4.串口        USART0:调试串口
//5.按键      
//6.OLED      硬件IIC
//7.MPU6050   硬件IIC
//8.串口屏
//9.陀螺仪串口

uint8_t beep_flag = 0;//声光提示标志位
uint8_t beep_key_flag;//按键提示音
uint8_t beep_timer = 0;//声光提示持续时间

uint8_t flag = 0;
char hmi_speed[]="n1.txt";//测速
char hmi_yaw[]="n2.txt";//测角度
char hmi_trace[]="n3.txt";//测循迹
char hmi_totol[]="n4.txt";//测里程
char hmi_imu[]="n5.txt";//测记忆角度
char data_HMI[]="";//串口屏数据存储空间
uint8_t Key_Timer;

uint32_t Timer_1ms_counter;
uint8_t System_Mode;

uint8_t WD_Protect_Flag;
uint8_t WD_Protect_time;
uint8_t WD_Protect_err;

float K_ZX = 0;
/*初始加速函数*/
void Speed_InitPro(uint8_t x)
{
    uint8_t temp;
    uint8_t i;
    for(i=0;i<x;i++)
    {
        Set_Speed(i,i);
    }
}

2. 主循环

执行系统所需要的初始化部分，注意初始化的顺序。

用于显示的部分放在主循环中。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
/*初始加速函数*/
void Speed_InitPro(uint8_t x)
{
    uint8_t temp;
    uint8_t i;
    for(i=0;i<x;i++)
    {
        Set_Speed(i,i);
    }
}

/*开启系统时钟中断，会导致串口工作异常或者程序卡死*/
int main(void)
{
    SYSCFG_DL_init();//syscfg初始化
    delay_ms(100);//等待初始化稳定
    OLED_Init();//OLED初始化
    OLED_ShowString(16,8,"OK",8);
    delay_ms(100);//等待陀螺仪稳定
    Motor_Off();//初始电机关闭，等待按键控制
    
    SYS_UART0_Init();//串口0初始化
    Screen_UART2_Init();//串口2初始化

    BP_Encoder_Init();//编码器初始化
    PID_Init();
    delay_ms(1000);//等待初始化稳定
    //Speed_InitPro(Speed_midset*0.8);

	//编码器中断使能
    delay_ms(50);//等待初始化稳定
    TIMER_1_Init();//定时器1初始化
    TIMER_2_Init();

    NVIC_EnableIRQ(UART_JY61P_INST_INT_IRQN);  
    while (1) 
    { 
        Oled_Proc();
        HMI_Proc();
    }
}

维特陀螺仪

一、通信协议

1.1 读格式

代码实现（仅读取角度）：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
// 定义接收变量
uint8_t RollL, RollH, PitchL, PitchH, YawL, YawH, VL, VH, SUM;
float Pitch,Roll,Yaw;

// 串口接收状态标识
#define WAIT_HEADER1 0
#define WAIT_HEADER2 1
#define RECEIVE_DATA 2

uint8_t RxState_JY = WAIT_HEADER1;
uint8_t receivedData[9];//接收数据储存数组
uint8_t dataIndex = 0;

// 串口中断处理函数
void UART_JY61P_INST_IRQHandler(void) {
    uint8_t uartdata = DL_UART_Main_receiveData(UART_JY61P_INST); // 接收一个uint8_t数据
    switch (RxState_JY) {
    case WAIT_HEADER1:
        if (uartdata == 0x55) { //协议头
            RxState_JY = WAIT_HEADER2;
        }
        break;
    case WAIT_HEADER2:
        if (uartdata == 0x53) { //数据内容：角度
            RxState_JY = RECEIVE_DATA;
            dataIndex = 0;
        } else {
            RxState_JY = WAIT_HEADER1; // 如果不是期望的第二个头，重置状态
        }
        break;
    case RECEIVE_DATA:
        receivedData[dataIndex++] = uartdata;
        if (dataIndex == 9) {
            // 数据接收完毕，分配给具体的变量
            RollL = receivedData[0];
            RollH = receivedData[1];
            PitchL = receivedData[2];
            PitchH = receivedData[3];
            YawL = receivedData[4];
            YawH = receivedData[5];
            VL = receivedData[6];
            VH = receivedData[7];
            SUM = receivedData[8];
            // 校验SUM是否正确
            uint8_t calculatedSum = 0x55 + 0x53 + RollH + RollL + PitchH + PitchL + YawH + YawL + VH + VL;
            if (calculatedSum == SUM) {
                // 校验成功，可以进行后续处理
            } else {
                // 校验失败，处理错误
            }
            RxState_JY = WAIT_HEADER1; // 重置状态以等待下一个数据包
        }
        break;
    }
//DL_UART_Main_transmitData(UART_JY61P_INST, uartdata); // 可选：回传接收到的数据
}

1.2 写格式

代码实现（Z轴置零）：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
//发送置偏航角置零命令
void Serial_JY61P_Zero_Yaw(void){

    /*解锁指令*/
    DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XFF);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XAA);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X69);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X88);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XB5);

	delay_ms(200);//延迟200ms

    /*修改指令（Z轴置零）*/
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XFF);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XAA);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X01);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X04);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);

	delay_ms(3000);//延迟3秒

    /*保存指令*/
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XFF);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XAA);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
	
}