我要评论目录
前言：
之前买了一批5路灰度传感器，想用这传感器进行循迹，无奈网上和官方的资料提供的还是比较少，这里还是做一下当初的学习记录。
小车效果展示：
stm32rct6主控，5路灰度寻迹，超声波hc_sr04中断式测距，蓝牙模块hc_08通信，as4950电机驱动芯片，减缓了mpu6050零漂问题，pid丝滑_哔哩哔哩_bilibili
5路数字灰度传感器：
这是某宝上买的5路灰度传感器，价格稍微有点贵，50多块一个。当然，一分钱一分货，这个模块可以用小螺丝刀来调节传感器上面的旋钮，通过这个旋钮来调节灵敏度，这个灵敏度调节好了的话可以识别黑白循迹、红白循迹。所以还是比较值得的。
对于这款5路数字灰度传感器来说。识别到黑线，传回来的数字量就是0，识别到白色，传回来的数字量就是1。
巡线思路：
博主想着用这5路的中间三路来巡线，用最左和最右端的传感器用来识别十字或者丁字路口，巡线时，加入pid算法，遇到十字或者丁字路口就用最左和最右的传感器来识别，识别到了之后，我们就可以搭配mpu6050进行转90°弯了。
加入pid调节的代码：
其中 sensor_bias 是根据中间三路传感器和黑线的相对位置来估计出的误差（如果你要问我怎么得来的，其实这个数据大差不差就行，它只是为pid服务的一个变量罢了，甚至你可以把62.5改成50，最后只要调好pid三个参数，达到的效果是一样的），这里的decide类似于状态机的信号，我令decide为6的时候，也就是小车跑出了黑线，小车停止。
#include "sensor.h"
#include "stm32f10x.h"
#include "move.h"
#include "motor.h"
#include "fsm.h"
//steer4 --> pa11 --> r2 红线
//steer3 --> pc9 --> r1 橘线
// --> pb4 --> m0 黄线
//steer1 --> pa6 --> l1 绿线
//encode1_a --> pb5 --> l2 棕线
float kp_sensor = 8.134, ki_sensor = 0.021, kd_sensor = 2.36;//pid弯道参数参数
float sensor_bias = 0;
float sensor_bias_last = 0;
float p = 0, i = 0, d = 0, pid_value = 0; //pid直道参数
int decide;
unsigned char move_flag;
extern unsigned char fsm_state;
void sensor_init()
{
gpio_inittypedef gpio_initstructure;
rcc_apb2periphclockcmd(rcc_apb2periph_gpioc,enable);//开启c时钟 pc9
gpio_initstructure.gpio_mode = gpio_mode_in_floating;
gpio_initstructure.gpio_pin = gpio_pin_9;
//gpio_initstructure.gpio_speed = gpio_speed_50mhz;//这句话其实可以不用，在使用输入功能时，不需要配置频率
gpio_init(gpioc, &gpio_initstructure);
rcc_apb2periphclockcmd(rcc_apb2periph_gpioa,enable);//开启a时钟 pa11 pa6
gpio_initstructure.gpio_mode = gpio_mode_in_floating;
gpio_initstructure.gpio_pin = gpio_pin_11|gpio_pin_6;
gpio_init(gpioa, &gpio_initstructure);
gpio_pinremapconfig(gpio_remap_swj_nojtrst, enable);
rcc_apb2periphclockcmd(rcc_apb2periph_gpiob,enable);//开启b时钟 pb4 pb5
gpio_initstructure.gpio_mode = gpio_mode_in_floating;
gpio_initstructure.gpio_pin = gpio_pin_4|gpio_pin_5;
gpio_init(gpiob, &gpio_initstructure);
}
unsigned char times;
extern unsigned char fsm_hc08;
unsigned char back_flag;
unsigned char one_time;
unsigned char one_flag;
unsigned char channel_num;//channel的存在导致发送2号通道的时候只会进入一次
void sensor_read()
{
if((l2 == 1)&&(l1 == 1)&&(m0 == 1)&&(r1 == 0))// 1 1 1 0
{
if(one_time == 0)
{
if(fsm_hc08 == channel_1)
{
fsm_state = turn_lift_state;//跳出循环，将pid分别清零。
one_time++;
p = 0;i = 0; d = 0;
}
else if(fsm_hc08 == channel_2)
{
if(one_flag == 0)
{
channel_num++;
one_flag = 1;
}
if(channel_num == 2)
{
fsm_state = stay2_state;//跳出循环，将pid分别清零。
one_time++;
p = 0;i = 0; d = 0;
}
}
}
}
else if((l1 == 0)&&(m0 == 1)&&(r1 == 0))// 0 1 0
{
sensor_bias = 0;decide = 3;one_flag = 0;//积分项清零
}
else if((l1 == 1)&&(m0 == 1)&&(r1 == 0))// 1 1 0
{
sensor_bias = -62.5;decide = 2;
}
else if((l1 == 0)&&(m0 == 1)&&(r1 == 1))// 0 1 1
{
sensor_bias = 62.5;decide = 2;
}
else if((l1 == 1)&&(m0 == 0)&&(r1 == 0))// 1 0 0
{
sensor_bias = -125;decide = 4;
}
else if((l1 == 0)&&(m0 == 0)&&(r1 == 1))// 0 0 1
{
sensor_bias = 62.5;decide = 4;
}
else if((l1 == 0)&&(m0 == 0)&&(r1 == 0))// 0 0 0
{
decide = 6;
}
else if((l1 == 1)&&(m0 == 1)&&(r1 == 1))// 1 1 1
{
decide = 6;fsm_state = judge_state; //如果读取到了整条黑线，那么就进入下一状态
if(back_flag == 1)
{
fsm_state = back_state;
back_flag = 0;
} //第一次识别到全黑线为小车停止线。第二次识别到，代表小车即将回归循迹
}
}
void sensor_pid()
{
if(decide<=5)
{
p = sensor_bias;
i = i + sensor_bias;
d = sensor_bias-sensor_bias_last;
pid_value = kp_sensor*p + ki_sensor*i + kd_sensor*d;
sensor_bias_last = sensor_bias;
//对积分值设置一个限制，防止积分值超标
if(i >=3500)i = 3500;
if(i <= -3500)i = -3500;
pwm_value_r = 2099 - (int)pid_value;
pwm_value_l = 2099 + (int)pid_value;//当线在左，左轮要慢，右轮要快，左轮要加，右轮要减，但这里的偏差是负值
motor3_forward(pwm_value_r);
motor4_forward(pwm_value_l);
//4 --> 右电机
//3 --> 左电机
}
else{
move_stop();
}
}
对应的头文件部分
#ifndef __sensor_h
#define __sensor_h
//steer4 --> pa11 --> r2 绿线
//steer3 --> pc9 --> r1 黄线
// b --> pb6 --> m0 橘线
//steer1 --> pa6 --> l1 红线
// a --> pb7 --> l2 白线
// 灰度传感器，当传感器识别到黑线的时候，输出为1，其余时刻输出为0
// 所以在这里我们要使用下拉输入
#define l2 gpio_readinputdatabit(gpiob,gpio_pin_5)
#define l1 gpio_readinputdatabit(gpioa,gpio_pin_6)
#define m0 gpio_readinputdatabit(gpiob,gpio_pin_4)
#define r1 gpio_readinputdatabit(gpioc,gpio_pin_9)
#define r2 gpio_readinputdatabit(gpioa,gpio_pin_11)
void sensor_init(void);
void sensor_read(void);
void sensor_pid(void);
#endif
发表评论