17年电赛的板球控制系统，PID算法的C程序实现+OpenMV的图像处理（颜色识别）

练习了17年电赛的板球控制系统，主要用到两个，一个是PID，另一个是OpenMV的图像处理（颜色识别）

PID:

程序参考这位大佬https://blog.csdn.net/qq_18454025/article/details/103443771

#include "main.h"
#include "stdio.h"

void PIDx_init(void);
float PIDx_realize(float speed);

void PIDy_init(void);
float PIDy_realize(float speed);

#include "PID.h"

	/*******************	X轴PID	********************************/
struct _pidx
{
    float SetSpeed;            //定义设定值
    float ActualSpeed;        //定义实际值
    float err;                //定义偏差值
    float err_last;            //定义上一个偏差值
    float Kp,Ki,Kd;            //定义比例、积分、微分系数
    float voltage;          //定义电压值（控制执行器的变量）
    float integral;            //定义积分值
}pidx;

void PIDx_init(void)
	{
    //printf("PID_init begin \n");
    pidx.SetSpeed=0.0;
    pidx.ActualSpeed=0.0;
    pidx.err=0.0;
    pidx.err_last=0.0;
    pidx.voltage=0.0;
    pidx.integral=0.0;
    pidx.Kp=2;
    pidx.Ki=0;
    pidx.Kd=0.2;
    //printf("PID_init end \n");
	}
	
float PIDx_realize(float speed)
	{
		extern uint16_t Camera_Ball_X; //摄像头下 滚球的位置
		extern uint16_t X_PID_P,X_PID_I,X_PID_D;
//		pidx.Kp=(float)X_PID_P/1000;
//		pidx.Ki=(float)X_PID_I/1000;
//		pidx.Kd=(float)X_PID_D/1000;/
		
		pidx.ActualSpeed=Camera_Ball_X;
    pidx.SetSpeed=speed;
    pidx.err=pidx.SetSpeed-pidx.ActualSpeed;
    pidx.integral+=pidx.err;
    pidx.voltage=pidx.Kp*pidx.err+pidx.Ki*pidx.integral+pidx.Kd*(pidx.err-pidx.err_last);
    pidx.err_last=pidx.err;
    pidx.ActualSpeed=(pidx.voltage-300)*(-0.5);
    return pidx.ActualSpeed;
	}

	/*******************	Y轴PID	********************************/
struct _pidy
{
    float SetSpeed;            //定义设定值
    float ActualSpeed;        //定义实际值
    float err;                //定义偏差值
    float err_last;            //定义上一个偏差值
    float Kp,Ki,Kd;            //定义比例、积分、微分系数
    float voltage;          //定义电压值（控制执行器的变量）
    float integral;            //定义积分值
}pidy;

void PIDy_init(void)
	{
    //printf("PID_init begin \n");
    pidy.SetSpeed=0.0;
    pidy.ActualSpeed=0.0;
    pidy.err=0.0;
    pidy.err_last=0.0;
    pidy.voltage=0.0;
    pidy.integral=0.0;
    pidy.Kp=2;
    pidy.Ki=0;
    pidy.Kd=0.2;
    //printf("PID_init end \n");
	}
	
float PIDy_realize(float speed)
	{
		extern uint16_t Camera_Ball_Y; //摄像头下 滚球的位置
		extern uint16_t Y_PID_P,Y_PID_I,Y_PID_D;
		
//		pidy.Kp=(float)Y_PID_P/1000;
//		pidy.Ki=(float)Y_PID_I/1000;
//		pidy.Kd=(float)Y_PID_D/1000;  //
		
		
		pidy.ActualSpeed=Camera_Ball_Y;
    pidy.SetSpeed=speed;
    pidy.err=pidy.SetSpeed-pidy.ActualSpeed;
    pidy.integral+=pidy.err;
    pidy.voltage=pidy.Kp*pidy.err+pidy.Ki*pidy.integral+pidy.Kd*(pidy.err-pidy.err_last);
    pidy.err_last=pidy.err;
    pidy.ActualSpeed=(pidy.voltage-300)*(-0.5);
    return pidy.ActualSpeed;
	}

OpenMV:

额，忘记了，程序存在了OpenMV的内存卡上了，OpenMV在学校，等开学再贴上吧。。