【STM32+HAL】杆球控制系统

一、前言

2017年电赛出了道板球控制系统题目，现写一个简化版本——杆球控制系统，以此记录电赛集训生活。

二、题目分析

最终采取的方案是：OpenMV读取小球的当前位置，并将坐标值传给STM32端，再由32通过电机改变杆的位置来改变乒乓球位置，由此实现闭环控制。模式间切换通过按键实现。

三、 所用工具

1、电机：DengFOC的2208无刷云台电机（无刷电机控制精度更高，算法较舵机难）

2、芯片：STM32F407ZGT6

3、机械结构：建议3D打印，提高精度，减轻算法压力。

四、CubeMX配置

1、控制+计时定时器

频率 frequency = 84MHz / 84 / 1000 = 1000Hz，即周期为1ms。

2、三路PWM输出定时器

FOC控制需三路PWM波，频率设为25kHz。

有关DengFOC的更多内容，请移步B站搜索DengFOC。

3、LCD屏显示配置

详见【STM32+HAL】LCD显示及触摸初始化配置，这里不再赘述。

4、读取AS5600编码器

配置IIC模式读取编码器值，获取当前电机转动角度。

5、串口配置

串口一通过DMA与OpenMV通信，有关串口DMA传输的内容，详见【STM32+HAL】DMA应用

至此，CuebMX配置完毕。

五、OpenMV识别

代码不难，大家自行理解吧。想了解更多OpenMV技术细节详见【OPENMV】学习记录 (持续更新)

import sensor, image, time, ustruct
from pyb import UARTuart = UART(3, 115200, timeout_char=200)
uart.init(115200, bits=8, parity=None, stop=1)  # init with given parameters
roi = (30, 50, 126, 22)# 初始化摄像头
sensor.reset()                        # 初始化感应器
sensor.set_pixformat(sensor.RGB565)   # 设置像素格式为RGB565
sensor.set_framesize(sensor.QQVGA)     # 设置帧大小为320x240
sensor.set_brightness(-3)
sensor.set_contrast(-3)
sensor.set_saturation(-3)
sensor.set_windowing(roi)
sensor.skip_frames(time = 2000)       # 跳过前2秒帧，用于摄像头设置稳定
sensor.set_auto_gain(False)           # 必须关闭才能进行颜色跟踪
sensor.set_auto_whitebal(False)       # 必须关闭才能进行颜色跟踪
clock = time.clock()                  # 初始化时钟对象def send_data(x):global uart;uart.write(str(x))uart.write(bytearray([0x20]))# 设置颜色阈值，识别黄色
yellow_threshold = (80, 100, -35, 29, 32, 100)
white_threshole = (90,100,-2,2,-2,2)while(True):clock.tick()                      # 开始新帧计时sensor.set_hmirror(True)img = sensor.snapshot().lens_corr(1.9).mean(1)           # 捕获图像blobs = img.find_blobs([yellow_threshold])if blobs:largest_blob = max(blobs, key=lambda b: b.pixels())# 绘制找到的物体边界框img.draw_rectangle(largest_blob.rect(), color=(255, 0, 0))img.draw_cross(largest_blob.cx(), largest_blob.cy(), color=(255, 0, 0))send_data(largest_blob.cx())

六、Keil填写代码

1、电机控制

此文件包含两个函数：

PID_Control：控制乒乓球运动到指定位置

PID_Sin：控制乒乓球以杆中心做正弦运动

有关无刷电机驱动的内容，详见[STM32+HAL]DengFOC移植之闭环速度控制

#include "Control.h"
#include "tim.h"
#include "FOC2.h"#define KP 	0.21			// 比例系数
#define KI 	0.000022		// 积分系数
#define KD  13       		// 微分系数double Current_position;			//小球当前位置
double Target_position;			//小球目标位置/* 定点PID控制函数 */
float PID_Control(float Current,float Target)
{double Error, Integral, Derivative, LastError;/*PID算法*/Error = Target - Current;								// 当前误差Integral += Error;Derivative = Error - LastError;							// E[k]-E[k-1]项/*更新输出值*/float temp = KP * Error + (KI) * Integral + KD * Derivative;temp = (temp > 20 ) ? 20 : (temp < -20) ? -20 :temp;	//限幅LastError = Error;										// 存储误差，用于下次计算return temp;
}/* Sin运动PID控制函数 */
float PID_Sin(float Current,float Target)
{double Error1, Integral1, Derivative1, LastError1;	/*Sin波的PID算法*/float P = 0.5;float D = 3;Error1 = Target - Current;								// 当前误差Derivative1 = Error1 - LastError1;			/*更新输出值*/float temp = P * Error1 + D * Derivative1;temp = (temp > 20 ) ? 20 : (temp < -20) ? -20 :temp;	//限幅LastError1 = Error1;return temp;
}

2、LCD显示

/* 界面一： 显示小球目标与当前位置曲线 */
void Show_1(uint8_t flag)
{LCD_ShowString(20,160,100,16,16,"Distance:");LCD_ShowString(160,160,100,16,16,"target_cx:");LCD_ShowString(20,210,100,16,16,"Overshoot:");LCD_ShowString(160,210,100,16,16,"Time:");if(fabs(target_cx - cx) < 2 && flag_Overshoot == 0)		//第一次经过目标值时，重置超调量，改变标志位{flag_Overshoot = 1;Max_Overshoot = 0;Overshoot = 0;}if(flag_Overshoot == 1)									//判断已经过了一次目标值后的最大偏移{Overshoot = fabs(target_cx - cx);Max_Overshoot = (Max_Overshoot <= Overshoot) ? Overshoot :Max_Overshoot;	//取最大值}LCD_ShowNum(95,160,distance,3,16);LCD_ShowNum(240,160,distance_target,3,16);LCD_ShowNum(103,210,(Max_Overshoot - 8.057) / 3.4615,3,16);	//距离值与像素转换LCD_ShowNum(240,210,time,5,16);draw_distance_wave1(distance,distance_target);			//绘制小球位置曲线
}/* 界面二：两种模式之间切换界面 */
void Show_2(uint8_t flag)
{LCD_ShowString(20,160,100,16,16,"Left point:");LCD_ShowNum(105,160,2,3,16);LCD_ShowString(175,160,100,16,16,"Right point:");LCD_ShowNum(270,160,30,3,16);LCD_ShowString(20,30,100,16,16,"Set Cycle:");LCD_ShowNum(115,30,3,2,16);
}/* 界面三：Sin运动曲线 0 - 30cm */
void Show_3(uint8_t flag)
{LCD_ShowString(20,210,100,16,16,"Left point:");LCD_ShowString(160,210,100,16,16,"Right point:");LCD_ShowString(160,30,100,16,16,"Fact Cycle:");LCD_ShowNum(115,210,2,3,16);LCD_ShowNum(255,210,30,3,16);LCD_ShowNum(255,30,3,3,16);draw_distance_wave(distance);			//绘制小球位置曲线
}/* 显示的总菜单 */
void Show(uint8_t flag)
{if(flag != last_flag) LCD_Clear(WHITE);switch (flag){case 1:Show_1(flag),HAL_TIM_Base_Start_IT(&htim2);	//界面一，定点控制break;case 2:Show_2(flag),HAL_TIM_Base_Stop_IT(&htim2);	//界面二，两种模式停顿切换界面break;case 3:Show_3(flag),HAL_TIM_Base_Start_IT(&htim2);	//界面三，Sin运动曲线控制break;default:break;}last_flag = flag;
}

3、mian.c

为缩减篇幅，下附代码删减了部分冗余的代码，相信以大家的聪明才智一定看得懂！

思路：初始化后，当按键按下切换模式后，显示对应的界面，并在定时器回调函数中进行一系列的控制与计时。

int main(void)
{/* USER CODE BEGIN 2 *//* OpenMV接收数据初始化 */__HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);HAL_UART_Receive_DMA(&huart1,rx_buffer,RXBUFFERSIZE);/* FOC初始化 */DFOC_Vbus(VMax);DFOC_alignSensor(PP,DIR);HAL_Delay(200);/* LCD初始化 */Show_Init();/* USER CODE END 2 *//* Infinite loop *//* USER CODE BEGIN WHILE */while (1){Show(flag);/* USER CODE END WHILE *//* USER CODE BEGIN 3 */}/* USER CODE END 3 */
}/* USER CODE BEGIN 4 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)		    //1ms定时器
{	/* 电机定时控制 */if( htim -> Instance == TIM2){/* flag == 1时进入闭环位置控制 */if(flag == 1){if(flag_time_loop == 0) time_loop++;					//若不为目标位置，时间周期++time = 1.5 * time_loop;									//时间转换target_cx = distance_target * 3.4615 + 8.057;DFOC_M0_set_Force_Angle(PID_Control(cx,target_cx));		//设置位置}/* 当flag == 3时进入曲线控制 */else if(flag == 3){DFOC_M0_set_Force_Angle(PID_Sin(cx,60));				//设置位置}else{time_loop = 0;flag_time_loop = 0;}}
}/* 按键消抖 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{if(HAL_GPIO_ReadPin(KEY0_GPIO_Port,KEY0_Pin) == GPIO_PIN_RESET){HAL_Delay(20); //延时消抖if(HAL_GPIO_ReadPin(KEY0_GPIO_Port,KEY0_Pin) == GPIO_PIN_RESET){/* flag切换模式 */flag = (flag + 1) % 4;}}else if(HAL_GPIO_ReadPin(KEY1_GPIO_Port,KEY1_Pin) == GPIO_PIN_RESET){HAL_Delay(20); //延时消抖if(HAL_GPIO_ReadPin(KEY1_GPIO_Port,KEY1_Pin) == GPIO_PIN_RESET){/* distance = 30 切换距离 */distance_target = (distance_target + 1) % 31;}}
}
/* USER CODE END 4 */

七、源码提供

夸克：我用夸克网盘分享了「Ball_Control」，点击链接即可保存。

百度：通过百度网盘分享的文件 提取码：6666

Gitee：Ball_Control

CSDN：Ball_Control

八、成果欣赏

Ball_Control

九、结语

本人能力有限，代码未必是最优解，若有可改进之处望在评论区留言。

如有小伙伴想交流学习心得，欢迎加入群聊751950234，群内不定期更新代码，以及提供本人博客所有源码