一、前言
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 UART
uart = 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」，点击链接即可保存。
Gitee：Ball_Control
CSDN：Ball_Control
八、成果欣赏
Ball_Control
九、结语
本人能力有限，代码未必是最优解，若有可改进之处望在评论区留言。
如有小伙伴想交流学习心得，欢迎加入群聊751950234，群内不定期更新代码，以及提供本人博客所有源码
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
