点光源追踪系统实现

点光源追踪系统实现

全国大学生电子设计竞赛只剩下不到二十天了，为了适应电赛四天三夜紧张的环境和磨合磨合队员之间的默契度，和小组三人用三天时间完成了点光源系统的制作这次模拟竞赛。系统以STM32单片机为控制核心，先通过K60单片机和鹰眼摄像头采集点光源的位置信息，再由串口传输给STM32单片机，STM32单片机产生控制信号传递给舵机，舵机带动激光笔实现点光源的实时检测及精确追踪。

确定方案

1.点光源的捕捉
系统的重点在于对于点光源信号的捕捉，考虑到光敏电阻传感器和摄像头视觉模块两种方案，光敏电阻传感器虽然可以对光源信号强弱实现模拟量输出，但对于这个系统可能会有较大的误差，导致输出模拟量难以处理，所以决定用摄像头模块直接捕获点光源的的位置。摄像头使用的是鹰眼OV7725,采集速度高达 150 帧每秒, 且采用硬件二值化。使用主频高达150M的山外K60核心板来驱动摄像头，我们使用60*80的分辨率，将捕获到的点光源坐标利用串口发送到我的32战舰板实现两块主控的通信。调整好摄像头的焦距与阈值，经过测试可以非常好的实现1W白光LED光源的捕获。

2.电机选择
由于要用激光笔指向点光源，激光笔要实现xy轴的二维运动，考虑到了使用步进电机与舵机，最后选择了两个舵机，主要原因还是舵机响应速度快，控制起来也很方便。舵机在转动过程中会产生较大的干扰，它可能通过电源或电源地来影响单片机的正常工作，加入光耦隔离后，信号可以传输，而干扰又不能通过（光隔是单向传输）。故能有效抑制系统噪声，消除接地回路的干扰。两路光耦隔离电路如图 所示。

3.算法设计
在本系统中，对于激光灯的运动过程有着严格的要求。所以在控制激光灯的时候就需要很精确的控制，在一系列的讨论之后，确定了选用最常用的PID控制算法。该调节器具有误差自动校正功能。其公式如下

实验过程

我们将两个舵机固定好，可以实现在水平X与上下Y方向的运动，激光笔固定在y方向转动的舵机上，y方向转动的舵机固定在x方向转动的舵机上，第一次是将摄像头与x方向转动的舵机的角度中值处对齐固定死,结果发现没法形成闭环控制，因为由于我们的摄像头只能检测到led点光源，而无法捕获到激光笔的坐标，从而在控制过程中激光笔是否精确的指向了点光源是没有反馈的，也就是没法确定激光笔是否在控制后达到了我所期望的目标值。最后我们改变了摄像头的位置，将其与激光笔平行固定在一起，让摄像头随激光笔一起同步运动，这样激光笔就与摄像头是相对静止的。由于检测不到激光笔的红点我们先将白色点光源移动到激光笔位置，读取激光笔在摄摄象头视野（60*80）里的坐标，此值就是我们的期望值（我的x：42/y : 25）。如若点光源移动，其坐标变化，就会产生反馈输出控制舵机运动，这样闭环就形成了。

算法代码

控制部分在定时器中断里面：

#include "control.h"
#include "led.h"
#include "servo.h"
#include "lcd.h"
#include "SERIAL.h"
#include "page.h"
#include "key.h"
PID_TYPE X_PID;
PID_TYPE Y_PID;
UINT8_XY XY_Data;
float PID_X_Position(float SetValueX ,float SensorValueX);
float PID_Y_Position(float SetValueY ,float SensorValueY);
void Motordrive(u16 *pwmx,u16 *pwmy);
 /******************************************************************
*函数名称: SetParameterXY
*函数功能: 参数化设计
*函数参数: 无
*返 回 值: 无
*******************************************************************/
void SetParameterXY(PID_TYPE *SETPID,float p,float i,float d)
{
 SETPID->P = p;
 SETPID->I = i;
 SETPID->D = d;
}
 /******************************************************************
*函数名称: TIM6_IRQHandler
*函数功能: 定时器6的更新中断,5ms
*函数参数: 无
*返 回 值: 无
*******************************************************************/
void TIM6_IRQHandler(void)
{  
 static u16 time1 = 0,time2 = 0;     //呼吸灯计数变量
 static u16 OutputX ,OutputY;
 const u8 Benchmark_X = 42,Benchmark_Y = 25;//基准值
 if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)        //检查指定的TIM中断发生与否:TIM中断源
 {
  switch(Page_Flag)
  {
   case 7:{//基本要求二
       SetParameterXY(&X_PID,0.3,0.003,0.1);
       SetParameterXY(&Y_PID,0.3,0.003,0.1);

       OutputX = MEDIAN_X - PID_X_Position(Benchmark_X,XY_Data.X);
       OutputY = MEDIAN_Y - PID_Y_Position(Benchmark_Y,XY_Data.Y);

       Motordrive(&OutputX,&OutputY);
       }break;
   case 13:{//其他部分，正方形，边长为9
         static u16 length = 2,i = 0,GoalX ,GoalY ;
        static u16 CoreX = MEDIAN_X,CoreY = MEDIAN_Y;
        const float Interval = 50;
         static u16 T1,T2,Makesure_Point = 0;//是否确定点的标志位
         
       time1++;                 //呼吸灯指示运行状态
       if (time1 > 49){time1 = 0;LED1 = !LED1;}  
            if(Key1Down()  == 1)
        {
         length +=  1;
         if(length == 10)length = 10;
        }
        if(Key0Down()  == 1)
        {
         length -=  1;
         if(length == 1)length = 1;
        }
        if(XY_Data.X >= 41 && XY_Data.X <= 43 && XY_Data.Y >= 24 && XY_Data.Y <= 26 )//是否确定,要精确，尽量减少误判
        {
         T1++;
         if(T1 > 0 && !(XY_Data.X >= 41 && XY_Data.X <= 43 && XY_Data.Y >= 24 && XY_Data.Y <= 26 ))//判断连续加持续200*5ms
         {
          T1 = 0;
          Makesure_Point = 0;
         }
         if(T1 > 200)
         {
          T1 = 0;
          Makesure_Point = 1;
         }
        } 
        if((Makesure_Point == 1)&&(T2 > 200))//等待300*5ms到可以框定目标
        {
         T2 = 0;
         if(XY_Data.X >= 50 || XY_Data.X <= 36 || XY_Data.Y >= 32 || XY_Data.Y <= 20 )
         {
          Makesure_Point = 0;
         }
        }
         if(Makesure_Point == 0)//确定点之前
        {
         SetParameterXY(&X_PID,0.3,0.003,0.1);
         SetParameterXY(&Y_PID,0.3,0.003,0.1);
         OutputX = MEDIAN_X - PID_X_Position(Benchmark_X,XY_Data.X);
         OutputY = MEDIAN_Y - PID_Y_Position(Benchmark_Y,XY_Data.Y);
         Motordrive(&OutputX,&OutputY);
        }

        if(Makesure_Point == 1)//确定到点画
        {
           T2++;
            CoreX = OutputX;
            CoreY = OutputY;        

	          i++;
	          if(i < Interval)       {GoalX = CoreX - length;GoalY = CoreY + length;}
	          else if(i < 2*Interval){GoalX = CoreX + length;GoalY = CoreY + length;}
	          else if(i < 3*Interval){GoalX = CoreX + length;GoalY = CoreY - length;}
	          else if(i < 4*Interval){GoalX = CoreX - length;GoalY = CoreY - length;}
	          else {i = 0;}

          Motordrive(&GoalX,&GoalY);
        
          time2++;                 //呼吸灯指示运行状态
          if (time2 > 25){time2 = 0;LED0 = !LED0;}  
        } 
    }
    break;
  TIM_ClearITPendingBit(TIM6, TIM_IT_Update);         //清除TIMx的中断待处理位:TIM中断源
 }
}

        

/******************************************************************
*函数名称:     PID_X_Position
*函数功能:     X轴位置式PID
*函数参数:   点光源当前x坐标/激光笔当前x坐标
*******************************************************************/
float PID_X_Position(float SetValueX ,float SensorValueX)
{
 X_PID.Error = SetValueX - SensorValueX;
 X_PID.Integral += X_PID.Error;
 X_PID.Differ = X_PID.Error - X_PID.LastError;
 
// if(X_PID.Integral > 2300)X_PID.Integral = 2300;
// if(X_PID.Integral < -2300)X_PID.Integral = -2300;
 
 X_PID.OutPut = (   X_PID.P * X_PID.Error
          + X_PID.I * X_PID.Integral
          + X_PID.D * X_PID.Differ    );
 
 X_PID.LastError = X_PID.Error;
 
 return X_PID.OutPut;
 
}

/******************************************************************
*函数名称:     PID_Y_Position
*函数功能:     Y轴位置式PID
*函数参数:   点光源当前y坐标/激光笔当前y坐标
*******************************************************************/
float PID_Y_Position(float SetValueY ,float SensorValueY)
{
 Y_PID.Error = SetValueY - SensorValueY;
 Y_PID.Integral += Y_PID.Error;
 Y_PID.Differ = Y_PID.Error - Y_PID.LastError;
 
// if(Y_PID.Integral > 2300)Y_PID.Integral = 2300;
// if(Y_PID.Integral < -2300)Y_PID.Integral = -2300;
 
 Y_PID.OutPut = (   Y_PID.P * Y_PID.Error
          + Y_PID.I * Y_PID.Integral
          + Y_PID.D * Y_PID.Differ    );
 
 Y_PID.LastError = Y_PID.Error;
 
 return Y_PID.OutPut;
 
}
/******************************************************************
*函数名称:     Motordrive
*函数功能:     驱动电机
*函数参数:   X/Y轴方向PID反馈输出值
*******************************************************************/
void Motordrive(u16 *pwmx,u16 *pwmy)
{
  if(*pwmx > 100) *pwmx = 100;
   if(*pwmx < 60) *pwmx = 60;
  if(*pwmy > 100) *pwmy = 100;
   if(*pwmy < 50) *pwmy = 50;
 
  SERVO_X = *pwmx;
   SERVO_Y = *pwmy;
}
case 7: 实现点光源的追踪
case 13：实现点光源的捕获后以光源为中心画正方形

头文件：
#ifndef __CONTROL_H
#define __CONTROL_H
#include "sys.h"
#define MEDIAN_X 80
#define MEDIAN_Y 77
//PID算法的数据结构 
typedef struct PID
{
  float P;         //参数
  float I;
  float D;
  float Error;     
  float Integral;  
  float Differ;    
  float LastError;
 
  float Ilimit; 
  float Irang;
  float OutPut;   
}PID_TYPE;   
//XY方向浮点型
typedef struct
{
 uint8_t X;
 uint8_t Y;
}UINT8_XY;
extern PID_TYPE X_PID;
extern PID_TYPE Y_PID;
extern UINT8_XY XY_Data;


#endif

实物图片：

在这里插入图片描述

忘了拍视频，最后追踪效果还是非常不错的，误差极小，相应也十分的迅速。