四轮两驱小车(四):STM32驱动5路灰度传感器PID循迹

您所在的位置:网站首页 左轮右轮电视剧 四轮两驱小车(四):STM32驱动5路灰度传感器PID循迹

四轮两驱小车(四):STM32驱动5路灰度传感器PID循迹

2024-01-10 18:56| 来源: 网络整理| 查看: 265

目录

前言:

小车效果展示:

5路数字灰度传感器:

巡线思路:

加入PID调节的代码:

前言:

        之前买了一批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=3500)I = 3500; if(I 右电机 //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



【本文地址】


今日新闻


推荐新闻


CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3