五路灰度传感器学习HJ-XJ5

目录

一、注释： 

二、接线

三、代码

灰度传感器bsp_GraySensor.c

 灰度传感器部bsp_GraySensor.h

主函数main.h

主函数mian.c

---

 

一、注释： 

参数：pwm：最大速度值，P：比例系数

根据位置式离散PID公式 
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  
∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2···k;

pid巡线函数：track_PID

float   Proportion;             // 比例常数 Proportional Const
        float   Integral;          // 积分常数 Integral Const
        float   Derivative;      // 微分常数 Derivative Const
        int      Error1;            // Error[n-1]
        int      Error2;            // Error[n-2]
        int      iError;             // Error[n]

PID position         位置式

PID int MatOut    设定输出最大值

int Setpoint          PID控制器设定值

position.iError=Setpoint-CurrentPoint;    // 计算当前误差
position.Error_sum+=position.iError;
iIncpid=position.Proportion * position.iError                  // P
          +position.Integral * position.Error_sum               // I
          +position.Derivative * (position.iError-position.Error1); // D
position.Error1=position.iError;          // 存储误差，用于下次计算  
sensor  传感器

analog  模拟值

左偏设置值为正，右偏设置值为负

二、接线

五路灰度传感器----stm32
DND--------------GND
VCC--------------5V
L2---------------C13
L1---------------C4
M----------------E6
R1---------------E5
R2---------------A4

(可自行更改）

通讯协议

要想得到偏移量就必须使用串口传输的形式 

三、代码

灰度传感器bsp_GraySensor.c

#include "bsp_GraySensor.h"
#include "main.h"
#include "adc.h"
#include "control.h"

//ÓÃÀ´Ñ²ÏßµÄ '»Ò¶È´«¸ÐÆ÷' ºÍ ÓÃÀ´¼ì²âÊÇ·ñ·ÅÉÏÒ©Æ·µÄ 'ºìÍâ´«¸ÐÆ÷' µÄ´úÂ붼·ÅÔÚÕâ¸ö.cÎļþÀïÁË

u8 L2_Val, L1_Val, M_Val, R1_Val, R2_Val;
int Line_Num;


u16 ADC_Value;
float ADC_Volt;
u8 Load_flag =0;  // 0±íʾ»¹Ã»×ªÔØÒ©Æ·£¬±íʾתÔØÍê±Ï£¬-1±íʾҩƷËÍ

u8 LoadCount,NotLoadCount;

/*******************»Ò¶È´«¸ÐÆ÷ѲÏß*************/
//Ö»Òª°ÑËٶȻ·¿ØÖƺ㬲¹³¥Öµ¼´¿ÉÈ·¶¨¡£  ²»Í¬ËÙ¶ÈÖµÐèÒª²»Í¬µÄ²¹³¥Öµ¡£  ²âÊÔºÃÒ»¸ö×îºÃµÄ¡£  ²»Í¬µÄתËÙÒª¶ÔÓ¦²»Í¬µÄ²¹³¥ÏµÊý»òÊýÖµ
void Light_GoStraight_control(void)   //»Ò¶ÈѲÏßÖ±ÐÐ, ÐèÒªÓиöÅжÏÐèÒªÖ±Ðж೤¾àÀëµÄº¯Êý    //²¹³¥Òªµ½ÖмäµÄ´«¸ÐÆ÷²âµ½»Øµ½ÏßÉÏΪֹ
{
	  Get_Light_TTL();  
	
	//Èç¹ûÒª¸ü¼Ó¾«×¼µÄ¿ØÖÆ£¬Ö»ÒªÓÐÁËѲÏß»·£¬¶¼ÐèÒª¶ÔλÖýøÐв¹³¥¡£  ¿É·ñÖ±½Ó½« g_lMotorPulseSigma  ºÍg_lMotor2PulseSigma ½øÐÐÊýÖµ¼Ó¼õ£¿£¿
  
	//Èç¹ûÑ°Ö±ÏßÓõĴ«¸ÐÆ÷¿ÉÒÔ¶ÁÈ¡Ä£ÄâÁ¿µÄ»°£¬Èý¸ö´«¸ÐÆ÷¶¼¿ÉÒÔѲֱÏßѰͦÎȵÄ


		if(L1_Val == 0 &&  M_Val == 1 && R1_Val == 0 )  Line_Num =  0;
	  if(L1_Val == 0 &&  M_Val == 1 && R1_Val == 1) Line_Num =  400;
	  if(L1_Val == 0 &&  M_Val == 0 && R1_Val == 1 )  Line_Num =  500;
	  if(L1_Val == 1 &&  M_Val == 0 && R1_Val == 0) Line_Num = -500;   
    if(L1_Val == 1 &&  M_Val == 1 && R1_Val == 0) Line_Num = -400;	
	
//	 if(LineNumToggleFlag == 1)
//	 {
//		 Line_Num = - Line_Num;
//	 }
	
}


//°Ñ»Ò¶È´«¸ÐÆ÷µ±×÷Ö»Êä³ö¸ßµÍµçƽ¡£ ¸ßµçƽÊÇʶ±ðµ½ºìÏßÁË¡£
void Get_Light_TTL(void)  
{
	if(HAL_GPIO_ReadPin(L2_GPIO_Port, L2_Pin) == GPIO_PIN_SET){L2_Val = 1;} else {L2_Val = 0;}
	if(HAL_GPIO_ReadPin(L1_GPIO_Port, L1_Pin) == GPIO_PIN_SET){L1_Val = 1;} else {L1_Val = 0;}
	if(HAL_GPIO_ReadPin(M_GPIO_Port, M_Pin) == GPIO_PIN_SET)  {M_Val = 1;} else {M_Val = 0;}
	if(HAL_GPIO_ReadPin(R1_GPIO_Port, R1_Pin) == GPIO_PIN_SET){R1_Val = 1;} else {R1_Val = 0;}
	if(HAL_GPIO_ReadPin(R2_GPIO_Port, R2_Pin) == GPIO_PIN_SET){R2_Val = 1;} else {R2_Val = 0;}
}



/********¼ì²âÊÇ·ñתÔØÒ©Æ·*************/
void LoadOrNot()
{
			HAL_ADC_Start(&hadc1);      
			if(HAL_ADC_PollForConversion(&hadc1, 10) == HAL_OK)
			{
				ADC_Value = HAL_ADC_GetValue(&hadc1);
				ADC_Volt =  (float)ADC_Value*3.3/4096.0;
			}
			
			if(ADC_Volt <= 1.6)
			{
				NotLoadCount = 0;
				
				LoadCount++;
				if(LoadCount > 3) //3´Î³ÌÐòÖ´Ðе½ÕâÖ®ºóÔÙ´ÎÅжÏ
				{
					Load_flag = 1;
				}
			}
			else if(ADC_Volt >  1.6)
			{
				LoadCount = 0;
				
				if(Load_flag == 1)    //±ØÐëÏÈ×°ÔعýÒ©Æ·µÄÇé¿öϲÅÄÜÅжÏÊÇ·ñÄÃ×ßÒ©Æ·
				{
					NotLoadCount++;
					if(NotLoadCount > 3 )  //3´Î³ÌÐòÖ´Ðе½ÕâÖ®ºóÔÙ´ÎÅжÏ
					{
						Load_flag = 2;
					}
			  }
			}
}

 灰度传感器部bsp_GraySensor.h

#ifndef __GRAYSENSOR_H
#define __GRAYSENSOR_H
#include "bsp_sys.h"

extern u8 L2_Val, L1_Val, M_Val, R1_Val, R2_Val;

extern u16 ADC_Value;
extern float ADC_Volt;
extern u8  Load_flag;


extern int Line_Num;


void LoadOrNot(void);

void Get_Light_TTL(void);
void Light_GoStraight_control(void);


#endif

主函数main.h

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.h
  * @brief          : Header for main.c file.
  *                   This file contains the common defines of the application.
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy;