3软件系统——基于STM32的滚球控制系统实现
3软件系统 ——《基于STM32的滚球控制系统实现》
本系统采用STM32F103ZET6单片机为控制中心，利用OpenMV3智能摄像头、显示器、按键、MG996R 舵机、小球、万向节、平板支架构成滚球控制系统。
单片机利用摄像头采集到的数据，确定小球的位置坐标，通过PID闭环控制舵机PWM输出打角拉动支撑杆，控制平板倾斜度达到小球滚到指定区域停留等状态。
控制系统采用PID算法组成闭环控制系统具有较好的稳定性。
本章旨在陈列软件系统。
项目整体较简单，参考较多网上资源并加以实现，仅供参考。
1 系统程序设计
根据需求分析确定系统硬件、机械结构后，进行滚球控制系统程序设计。
主要包括主控芯片即单片机程序、图像信息处理模块即摄像头模块程序两主要部分。
1.1 程序设计概述系统算法流程如图4.1所示，单片机系统主函数进行串口通信获取摄像头传递回单片机的串口通信信息，并设置5ms中断函数，中断函数中内置滚球控制函数。
每过5ms进行一次中断处理，即PID控制算法运算、控制舵机角度等内容的运行，实现滚球的控制。
通过中断的方式，将二者功能模块剥离开，更有利于后续程序的开发、调试。
主控芯片结合摄像头传输回的串口信息，进行控制算法运算，实现滚球控制系统的实时控制，使得滚球按照意向运动，即程序具体实现为滚球运动控制功能。
图4.1%(cx,cy)
40. print(rx_buf)
41. uart.write(rx_buf+\r\n)
单片机主要程序
MAIN.C该函数部分实现滚球控制系统整体功能实现。
主函数运行串口通信函数，设置5ms中断执行MODE函数，进行PID调节的运算和处理，实现滚球控制系统。
1. #include "led.h"
2. #include "delay.h"
3. #include "sys.h"
4. #include "stdio.h"
5. #include "stdlib.h"
6. #include "ctype.h"
7. #include "usart.h"
8. #include "timer.h"
9. #include "key.h"
10. #include "pid.h"
11. #include "math.h"
12. /*-------------------------结构体变量------------------------*/
13. extern PIDTypdDef M1PID; //PID1
14. extern PIDTypdDef M2PID; //PID2
15.
16. u8 len; //长度变量
17. void MODE1(void) ; //初始静态位置
18. void MODE3(void) ; //控制小球从中到下再到右
19. void MODE5(void) ; //
20. void MODE0(void) ; //初始动态位置
21. int cx,cy; //坐标变量
22. char runmain = 1; //任务运行标识符
23. #define M1_PWMMAX 300 //舵机一角度限制最大值
24. #define M2_PWMMAX 300 //舵机二角度限制最大值
25. int i,PWM1=1281,PWM2=1801; //舵机初始化平衡角度
26. //------------------------------复位函数---------------------------------//
27. void MCU_Reset(void)
28. {
29. __set_FAULTMASK(1); // 关闭所有中断
30. NVIC_SystemReset(); // 复位
31. }
32. //------------------------------控制函数---------------------------------//
33. void kz_hs(int32_t M1pwm1,int32_t M2pwm2)
34. {
35. M1pwm1 = 1281 + M1pwm1; //控制左右
36. M2pwm2 = 1801 + M2pwm2; //控制前后
37. TIM_SetCompare1(TIM3,M1pwm1);
38. TIM_SetCompare2(TIM3,M2pwm2);
39. }
40. //-----------------------------模式一板球初始静态位置------------------- //
41. void MODE1(void)
42. {
43. TIM_SetCompare1(TIM3,1281);
44. TIM_SetCompare2(TIM3,1801);
45. }
46. //------------------------模式三控制小球从中到下再到右下--------------------//
47. void MODE3(void)
48. {
49. const float priod = 1500.0;
50. static uint32_t MoveTimeCnt = 0;
51. float Normalization = 0.0;
52. float set_x,set_y;
53. float hc_time = 300;
54. MoveTimeCnt += 5;
55.
56. if(MoveTimeCnt<=priod)
57. {
58.
59. Normalization = (float)MoveTimeCnt / priod;
60. if(Normalization>=1.0)
61. {
62. Normalization = 1.0;
63. }
64. set_x = 122.0 - (4.0 * Normalization);
65. set_y = 142.0 + (61 * Normalization) ;
66.
67. PID_M1_SetPoint(set_x);
68. PID_M1_SetKp(3.5);
69. PID_M1_SetKi(0.02);
70. PID_M1_SetKd(50);
71. PID_M2_SetPoint(set_y);
72. PID_M2_SetKp(3.5);
73. PID_M2_SetKi(0.02);
74. PID_M2_SetKd(50);
75. }
76. else if(MoveTimeCnt>(priod+hc_time))
77. {
78. Normalization = (float)(MoveTimeCnt-(priod+hc_time)) / priod;
79. if(Normalization>=1.0)
80. {
81. Normalization = 1.0;
82. }
83. set_x = 122.0 + (64.0 * Normalization);
84. set_y = 203.0 + (4 * Normalization) ;
85.
86. PID_M1_SetPoint(set_x);
87. PID_M1_SetKp(3.5);
88. PID_M1_SetKi(0.02);
89. PID_M1_SetKd(50);
90. PID_M2_SetPoint(set_y);
91. PID_M2_SetKp(3.5);
92. PID_M2_SetKi(0.02);
93. PID_M2_SetKd(50);
94. }
95. PWM1=PID_M1_PosLocCalc((float)cx);
96.
97. PWM2=PID_M2_PosLocCalc((float)cy);
98.
99. if(PWM1 > M1_PWMMAX) PWM1=M1_PWMMAX;
100.
101. if(PWM1 < -M1_PWMMAX) PWM1=-M1_PWMMAX;
102.
103. if(PWM2 > M2_PWMMAX) PWM2=M2_PWMMAX;
104.
105. if(PWM2< -M2_PWMMAX) PWM2=-M2_PWMMAX;
106.
107. kz_hs(PWM1,PWM2);//控制函数
108.
109. }
110. //-------------------------模式零板球初始动态位置------------------- //
111. void MODE0(void)
112. {
113. float set_x,set_y;
114.
115. set_x = 122 ;
116. set_y = 142 ;
117.
118. PID_M1_SetPoint(set_x);
119. PID_M1_SetKp(1);
120. PID_M1_SetKi(0.02);
121. PID_M1_SetKd(100.0);
122. PID_M2_SetPoint(set_y);
123. PID_M2_SetKp(1);
124. PID_M2_SetKi(0.02);
125. PID_M2_SetKd(100.0);
126. PWM1=PID_M1_PosLocCalc((float)cx);
127. PWM2=PID_M2_PosLocCalc((float)cy);
128.
129. if(PWM1 > M1_PWMMAX) PWM1=M1_PWMMAX;
130.
131. if(PWM1 < -M1_PWMMAX) PWM1=-M1_PWMMAX;
132.
133. if(PWM2 > M2_PWMMAX) PWM2=M2_PWMMAX;
134.
135. if(PWM2< -M2_PWMMAX) PWM2=-M2_PWMMAX;
136.
137. kz_hs(PWM1,PWM2);
138.
139. }
140. //------------------------------串口函数--------------------------------//
141. void uart()
142. {
143. if(USART_RX_STA&0x8000)
144. {
145. len=USART_RX_STA&0x3f;//得到此次接受到的数据长度
146. if(len == 7) //判断长度是否和发送一致确定数据的准确性
147. {
148. cx = (USART_RX_BUF[0]-0)*100 + (USART_RX_BUF[1]-0)*10 + (USART_RX_BUF[2]-0); //将字符串转换整数
149. cy = (USART_RX_BUF[4]-0)*100 + (USART_RX_BUF[5]-0)*10 + (USART_RX_BUF[6]-0);
150. }
151. USART_RX_STA=0;
152. }
153. }
154. //------------------------------主函数-----------------------------------//
155. int main(void)
156. {
157. KEY_Init(); //按键初始化
158. uart_init(115200); //串口初始化
159. delay_init(); //延时初始化
160. NVIC_Configuration(); //设置NVIC中断分组2：2位抢占优先级，2位响应优先级
161. LED_Init(); //LED端口初始化
162. TIM3_PWM_Init(3599,49); //PWM频率
163. TIM2_Int_Init(199,7199); //5ms一次中断
164. TIM_SetCompare1(TIM3,PWM1);//舵机1PWM
165. TIM_SetCompare2(TIM3,PWM2);//舵机2PWM
166. PID_M1_Init();
167. PID_M2_Init();
168. while(1)
169. {
170. uart(); //串口读取坐标函数
171. }
172. }
173. //------------------------------5ms中断函数-----------------------------//
174. void TIM2_IRQHandler(void) //TIM2中断
175. {
176.
177. if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否：TIM中断源
178. {
179. TIM_ClearITPendingBit(TIM2, TIM_IT_Update ); //清楚TIMx的中断待处理位：TIM中断源
180. if(runmain == 1)
181. {
182. MODE3();
183. }
184. }
185. }
PID.C该函数部分实现控制滚球控制系统舵机部分的控制算法主体结构，运用位置式PID调节算法，以摄像头反馈小球坐标（实际值）和预设理想坐标（期望值）之间的差值为偏差。
除了参数设置函数外，最重要的函数为PID_M1_PosLocCalc、PID_M2_PosLocCalc，负责实现算法的计算和结果输出。
1. #include "stdio.h"
2. #include "string.h"
3. #include "pid.h"
4. /*------------------------------------------
5. 声明变量
6. ------------------------------------------*/
7. PIDTypdDef M1PID;
8. PIDTypdDef M2PID;
//---------------------初始化M1PID、M2PID结构体变量------------------//
9. /*------------------------------------------
10. 函数功能：初始化M1PID结构体参数
11. 函数说明：
12. ------------------------------------------*/
13. Void PID_M1_Init(void)
14. {
15. M1PID.LastError = 0; //Error[-1]
16. M1PID.PrevError = 0; //Error[-2]
17. M1PID.Proportion = 0; //比例常数
18. M1PID.Integral = 0; //积分常数
19. M1PID.Derivative = 0; //微分常数
20. M1PID.SetPoint = 0; //期望值
21. M1PID.SumError = 0;
22. }
23. /*------------------------------------------
24. 函数功能：初始化M2PID结构体参数
25. 函数说明：
26. ------------------------------------------*/
27. void PID_M2_Init(void)
28. {
29. M2PID.LastError = 0; //Error[-1]
30. M2PID.PrevError = 0; //Error[-2]
31. M2PID.Proportion = 0; //比例常数
32. M2PID.Integral = 0; //积分常数
33. M2PID.Derivative = 0; //微分常数
34. M2PID.SetPoint = 0; //期望值
35. M2PID.SumError = 0; //积分项
36. }
37. //---------------------设置M1PID、M2PID期望值------------------//
38. /*------------------------------------------
39. 函数功能：设置M1PID期望值
40. 函数说明：
41. ------------------------------------------*/
42. void PID_M1_SetPoint(float setpoint)
43. {
44. M1PID.SetPoint = setpoint;
45. }
46. /*------------------------------------------
47. 函数功能：设置M2PID期望值
48. 函数说明：
49. ------------------------------------------*/
50. void PID_M2_SetPoint(float setpoint)
51. {
52. M2PID.SetPoint = setpoint;
53. }
54. //---------------------设置M1PID、M2PID比例系数------------------//
55. /*------------------------------------------
56. 函数功能：设置M1PID比例系数
57. 函数说明：浮点型
58. ------------------------------------------*/
59. void PID_M1_SetKp(float dKpp)
60. {
61. M1PID.Proportion = dKpp;
62. }
63. /*------------------------------------------
64. 函数功能：设置M2PID比例系数
65. 函数说明：浮点型
66. ------------------------------------------*/
67. void PID_M2_SetKp(float dKpp)
68. {
69. M2PID.Proportion = dKpp;
70. }
71. //---------------------设置M1PID、M2PID积分系数------------------//
72. /*------------------------------------------
73. 函数功能：设置M1PID积分系数
74. 函数说明：浮点型
75. ------------------------------------------*/
76. void PID_M1_SetKi(float dKii)
77. {
78. M1PID.Integral = dKii;
79. }
80. /*------------------------------------------
81. 函数功能：设置M2PID积分系数
82. 函数说明：浮点型
83. ------------------------------------------*/
84. void PID_M2_SetKi(float dKii)
85. {
86. M2PID.Integral = dKii;
87. }
88. //---------------------设置M1PID、M2PID微分系数------------------//
89. /*------------------------------------------
90. 函数功能：设置M1PID微分系数
91. 函数说明：浮点型
92. ------------------------------------------*/
93. void PID_M1_SetKd(float dKdd)
94. {
95. M1PID.Derivative = dKdd;
96. }
97. /*------------------------------------------
98. 函数功能：设置M2PID微分系数
99. 函数说明：浮点型
100. ------------------------------------------*/
101. void PID_M2_SetKd(float dKdd)
102. {
103. M2PID.Derivative = dKdd;
104. }
105. /*------------------------------------------
106. 函数功能：电机1位置PID计算
107. 函数说明：
108. ------------------------------------------*/
109. int32_t PID_M1_PosLocCalc(float NextPoint)
110. {
111. register float iError,dError;
112.
113. iError = M1PID.SetPoint - NextPoint; // 偏差
114. M1PID.SumError += iError; // 积分
115. if(M1PID.SumError > 3300.0) //积分限幅
116. M1PID.SumError = 3300.0;
117. else if(M1PID.SumError < -3300.0)
118. M1PID.SumError = -3300.0;
119. dError = iError - M1PID.LastError; // 当前积分
120. M1PID.LastError = iError;
121.
122. return(int32_t)( M1PID.Proportion * iError // 比例项
123. + M1PID.Integral * M1PID.SumError // 积分项
124. + M1PID.Derivative * dError); // 微分项
125. }
126.
127. /*------------------------------------------
128. 函数功能：电机2位置PID计算
129. 函数说明：
130. ------------------------------------------*/
131. int32_t PID_M2_PosLocCalc(float NextPoint)
132. {
133. register float iError,dError;
134.
135. iError = M2PID.SetPoint - NextPoint;
136. M2PID.SumError += iError;
137. if(M2PID.SumError > 3300.0)
138. M2PID.SumError = 3300.0;
139. else if(M2PID.SumError < -3300.0)
140. M2PID.SumError = -3300.0;
141. dError = iError - M2PID.LastError;
142. M2PID.LastError = iError;
143.
144. return(int32_t)( M2PID.Proportion * iError
145. + M2PID.Integral * M2PID.SumError
146. + M2PID.Derivative * dError);
147. }
TIMER.C
该部分函数利用STM32的时钟系统实现对滚球控制系统时钟函数的设置，主要分为两部分：TIM3对应输出不同占空比PWM波控制舵机角度变化；TIM2对应main.c函数中设置的5ms中断，即每过5ms打断主函数，进行一次中断函数内容的处理。
1. #include "timer.h"
2. #include "led.h"
3. #include "usart.h"
4. void TIM2_Int_Init(u16 arr,u16 psc)
5. {
6. TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
7. NVIC_InitTypeDef NVIC_InitStructure;
8. RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
9. TIM_TimeBaseStructure.TIM_Period = arr;
10. TIM_TimeBaseStructure.TIM_Prescaler =psc;
11. TIM_TimeBaseStructure.TIM_ClockDivision = 0;
12. TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
13. TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
14. TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE );
15. NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
16. NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
17. NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
18. NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
19. NVIC_Init(&NVIC_InitStructure);
20. TIM_Cmd(TIM2, ENABLE);
21. }
22. void TIM3_Int_Init(u16 arr,u16 psc)
23. {
24. TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
25. NVIC_InitTypeDef NVIC_InitStructure;
26. RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
27. TIM_TimeBaseStructure.TIM_Period = arr;
28. TIM_TimeBaseStructure.TIM_Prescaler =psc;
29. TIM_TimeBaseStructure.TIM_ClockDivision = 0;
30. TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
31. TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
32. TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE );
33. NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
34. NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
35. NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
36. NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
37. NVIC_Init(&NVIC_InitStructure);
38. TIM_Cmd(TIM3, ENABLE);
39. }
40. void TIM3_IRQHandler(void)
41. {
42. if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)
43. {
44. TIM_ClearITPendingBit(TIM3, TIM_IT_Update );
45. }
46. }
47. void TIM3_PWM_Init(u16 arr,u16 psc)
48. {
49. GPIO_InitTypeDef GPIO_InitStructure;
50. TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
51. TIM_OCInitTypeDef TIM_OCInitStructure;
52. RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
53. RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB RCC_APB2Periph_AFIO RCC_APB2Periph_GPIOA, ENABLE);
54. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0GPIO_Pin_1;
55. GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
56. GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
57. GPIO_Init(GPIOB, &GPIO_InitStructure);
58. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6GPIO_Pin_7;
59. GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
60. GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
61. GPIO_Init(GPIOA, &GPIO_InitStructure);
62. TIM_TimeBaseStructure.TIM_Period = arr;
63. TIM_TimeBaseStructure.TIM_Prescaler =psc;
64. TIM_TimeBaseStructure.TIM_ClockDivision = 0;
65. TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
66. TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
67. TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
68. TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
69. TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
70. TIM_OC1Init(TIM3, &TIM_OCInitStructure);
71. TIM_OC2Init(TIM3, &TIM_OCInitStructure);
72. TIM_OC3Init(TIM3, &TIM_OCInitStructure);
73. TIM_OC4Init(TIM3, &TIM_OCInitStructure);
74. TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
75. TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
76. TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
77. TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
78. TIM_Cmd(TIM3, ENABLE);
3软件系统 ——《基于STM32的滚球控制系统实现》
Comments
Post a Comment