SkyLi
122 lines
4.2 KiB
C
122 lines
4.2 KiB
C
#include "GrayscaleSensor.h"
|
|
#include "zf_driver_adc.h"
|
|
#include "zf_driver_gpio.h"
|
|
#include "zf_driver_delay.h"
|
|
|
|
#define AD2 B8
|
|
#define AD1 B9
|
|
#define AD0 B14
|
|
|
|
#define FILTER_ALPHA 0.5f //位置滤波系数，范围 [0, 1]，值越大响应越快但噪声越大
|
|
|
|
volatile static uint16 adc_white[8] = {4096,4096,4096,4096,4096,4096,4096,4096}; // 存储8路灰度传感器的白色参考值，初始值为满量程
|
|
volatile static uint16 adc_black[8] = {0,0,0,0,0,0,0,0}; // 存储8路灰度传感器的黑色参考值，初始值为0
|
|
volatile static uint16 adc_range[8] = {4096,4096,4096,4096,4096,4096,4096,4096}; // 存储8路灰度传感器的动态范围，初始值为满量程
|
|
volatile static uint16 adc_raw_value[8] = {0}; // 存储8路灰度传感器的原始 ADC 值
|
|
static float adc_normalized[8] = {0}; // 存储8路灰度传感器的归一化值，范围 [0, 1]
|
|
static float adc_normalized_sum = 0; // 归一化值的总和，用于计算位置
|
|
volatile float grayscale_sensor_position_prev = 0; // 存储小车相对于黑线的位置
|
|
volatile float grayscale_sensor_position = 0; // 存储小车相对于黑线的位置
|
|
static const int position[8] = {-4, -3, -2, -1, 1, 2, 3, 4}; // 每个传感器对应的位置
|
|
|
|
void grayscale_sensor_set_white(void)
|
|
{
|
|
for(uint8 i=0;i<8;i++)
|
|
{
|
|
adc_white[i] = adc_raw_value[i];
|
|
adc_range[i] = adc_white[i] - adc_black[i];
|
|
}
|
|
}
|
|
void grayscale_sensor_set_black(void)
|
|
{
|
|
for(uint8 i=0;i<8;i++)
|
|
{
|
|
adc_black[i] = adc_raw_value[i];
|
|
adc_range[i] = adc_white[i] - adc_black[i];
|
|
}
|
|
}
|
|
|
|
void grayscale_sensor_init(void)
|
|
{
|
|
adc_init(ADC1_CH5_B18, ADC_12BIT);
|
|
gpio_init(AD2, GPO, GPIO_LOW, GPO_PUSH_PULL);
|
|
gpio_init(AD1, GPO, GPIO_LOW, GPO_PUSH_PULL);
|
|
gpio_init(AD0, GPO, GPIO_LOW, GPO_PUSH_PULL);
|
|
gpio_set_level(AD2, 1);
|
|
gpio_set_level(AD1, 1);
|
|
gpio_set_level(AD0, 1);
|
|
}
|
|
/*真值表
|
|
AD2 AD1 AD0
|
|
0 0 0 CH1
|
|
0 0 1 CH2
|
|
0 1 0 CH3
|
|
0 1 1 CH4
|
|
1 0 0 CH5
|
|
1 0 1 CH6
|
|
1 1 0 CH7
|
|
1 1 1 CH8
|
|
*/
|
|
void grayscale_sensor_scan(void)
|
|
{
|
|
gpio_set_level(AD2, 0);
|
|
gpio_set_level(AD1, 0);
|
|
gpio_set_level(AD0, 0); // CH1
|
|
system_delay_us(1);
|
|
adc_raw_value[0]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
|
gpio_set_level(AD2, 0);
|
|
gpio_set_level(AD1, 0);
|
|
gpio_set_level(AD0, 1); // CH2
|
|
system_delay_us(1);
|
|
adc_raw_value[1]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
|
gpio_set_level(AD2, 0);
|
|
gpio_set_level(AD1, 1);
|
|
gpio_set_level(AD0, 0); // CH3
|
|
system_delay_us(1);
|
|
adc_raw_value[2]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
|
gpio_set_level(AD2, 0);
|
|
gpio_set_level(AD1, 1);
|
|
gpio_set_level(AD0, 1); // CH4
|
|
system_delay_us(1);
|
|
adc_raw_value[3]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
|
gpio_set_level(AD2, 1);
|
|
gpio_set_level(AD1, 0);
|
|
gpio_set_level(AD0, 0); // CH5
|
|
system_delay_us(1);
|
|
adc_raw_value[4]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
|
gpio_set_level(AD2, 1);
|
|
gpio_set_level(AD1, 0);
|
|
gpio_set_level(AD0, 1); // CH6
|
|
system_delay_us(1);
|
|
adc_raw_value[5]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
|
gpio_set_level(AD2, 1);
|
|
gpio_set_level(AD1, 1);
|
|
gpio_set_level(AD0, 0); // CH7
|
|
system_delay_us(1);
|
|
adc_raw_value[6]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
|
gpio_set_level(AD2, 1);
|
|
gpio_set_level(AD1, 1);
|
|
gpio_set_level(AD0, 1); // CH8
|
|
system_delay_us(1);
|
|
adc_raw_value[7]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
|
|
|
|
// 计算小车相对于黑线的位置
|
|
grayscale_sensor_position = 0;
|
|
adc_normalized_sum = 0;
|
|
for(uint8 i=0;i<8;i++)
|
|
{
|
|
adc_normalized[i] = ((float)(adc_white[i] - adc_raw_value[i])) / adc_range[i];
|
|
grayscale_sensor_position += adc_normalized[i] * position[i];
|
|
adc_normalized_sum += adc_normalized[i];
|
|
}
|
|
if(adc_normalized_sum > 0)
|
|
{
|
|
grayscale_sensor_position /= adc_normalized_sum;
|
|
}
|
|
else
|
|
{
|
|
grayscale_sensor_position = 0; // 避免除以零，默认位置为0
|
|
}
|
|
grayscale_sensor_position = grayscale_sensor_position_prev + FILTER_ALPHA * (grayscale_sensor_position - grayscale_sensor_position_prev);
|
|
grayscale_sensor_position_prev = grayscale_sensor_position;
|
|
} |