本文最后更新于335 天前，其中的信息可能已经过时，如有错误请发送邮件到273925452@qq.com
这是小鹿大二打智能车摄像头组时学习TC264记录的代码例程😊，历程很多官网都有，这里是做了一下整理，算法可能比较旧，后来改了代码后没有整理😭
BSP
KEY
//按键变量
#define KEY1 P22_0
#define KEY2 P22_1
#define KEY3 P22_2
#define KEY4 P22_3
//开关状态变量
uint8 key1_status=1,key2_status=2,key3_status=1,key4_status=1;
//上一次开关状态变量
uint8 key1_last_status,key2_last_status,key3_last_status,key4_last_status;
uint8 key1_flag,key2_flag,key3_flag,key4_flag;
uint8 a1=0,b1=0,c1=0,d1=0;
void Key_Init(void);
void Key_procces();
Key_Init();
while (TRUE)
{
Key_procces();
}
void Key_Init(void)
{
gpio_init(KEY1,GPI,0,PULLUP);
gpio_init(KEY2,GPI,0,PULLUP);
gpio_init(KEY3,GPI,0,PULLUP);
gpio_init(KEY4,GPI,0,PULLUP);
}
void Key_procces()
{
//保存按键状态
key1_last_status = key1_status;
key2_last_status = key2_status;
key3_last_status = key3_status;
key4_last_status = key4_status;
//读取当前按键状态
key1_status = gpio_get(KEY1);
key2_status = gpio_get(KEY2);
key3_status = gpio_get(KEY3);
key4_status = gpio_get(KEY4);
//检测到按键按下之后并放开 释放一次信号量
if(key1_status && !key1_last_status) key1_flag = 1;
if(key2_status && !key2_last_status) key2_flag = 1;
if(key3_status && !key3_last_status) key3_flag = 1;
if(key4_status && !key4_last_status) key4_flag = 1;
if(key1_flag)
{
key1_flag = 0;
a1+=1;
printf("a1=%d\n",a1);
}
if(key2_flag)
{
key2_flag = 0;
b1+=1;
printf("b1=%d\n",b1);
}
if(key3_flag)
{
key3_flag = 0;
c1+=1;
printf("c1=%d\n",c1);
}
if(key4_flag)
{
key4_flag = 0;
d1+=1;
printf("d1=%d\n",d1);
}
}
PWM
初始化
电机驱动用的
https://item.taobao.com/item.htm?spm=3178c.6639537/tb.1997196601.44.6a907484mISWHT&id=532985053318
#define R2 (ATOM0_CH2_P21_4)
#define R1 (ATOM0_CH3_P21_5)
#define L2 (ATOM0_CH0_P21_2)
#define L1 (ATOM0_CH1_P21_3)
pwm_init(L1, 17000, 0); // 初始化 PWM 通道 频率 17KHz 初始占空比 0%
pwm_init(L2, 17000, 0); // 初始化 PWM 通道 频率 17KHz 初始占空比 0%
pwm_init(R1, 17000, 0); // 初始化 PWM 通道 频率 17KHz 初始占空比 0%
pwm_init(R2, 17000, 0); // 初始化 PWM
//L
pwm_set_duty(L1, 0); // 更新对应通道占空比
pwm_set_duty(L2, 0);
//R
pwm_set_duty(R1, 0); // 更新对应通道占空比
pwm_set_duty(R2, 0);USART
#include "zf_uart.h"
uint8 a[5]={'0','0','0','0'};//串口测试数组
uint8 b[]="hello";//串口测试数组
uint8 c[10] ={'0','0','0','0'};
c[0] = 1+'0';
c[1] = 2+'0';
uart_init(UART_0,115200,UART0_TX_P14_0,UART0_RX_P14_1);
uart_putstr(UART_0,"Hello Heiweilu\r\n");//发送字符串
uart_putchar(UART_0, 0xA5);//发送字节A5
uart_putbuff(UART_0,&a[0],1);//发送数组，字符串
uart_putbuff(UART_0,&b,2);
uart_putbuff(UART_0,&c,2);
printf("printf demo\n");
//注意事项1 printf打印的数据会通过串口发送出去
//注意事项2 printf所使用的串口和波特率可以在TC264_config.h文件中修改
//注意事项3 printf所使用的串口默认在get_clk函数中进行了初始化，如果不想使用printf则可以在TC264_config.h文件中将PRINTF_ENABLE宏定义设置为0即可OLED
SPI or IIC通信
7pin
oled_init();
void OLED_Display()
{
oled_fill(0xff);
oled_p8x16str(20,4,'abc');
if(L1) oled_p8x16str(20,4,"0");
else oled_p8x16str(20,4,"1");
if(L2) oled_p8x16str(40,4,"0");
else oled_p8x16str(40,4,"1");
if(W) oled_p8x16str(60,4,"0");
else oled_p8x16str(60,4,"1");
if(R1) oled_p8x16str(80,4,"0");
else oled_p8x16str(80,4,"1");
if(R2) oled_p8x16str(100,4,"0");
else oled_p8x16str(100,4,"1");
}编码器
512线编码器
int16 speed1,speed2;
/***@brief: 【初始化编码器模块与引脚 带方向增量编码器模式】***/
encoder_dir_init(TIM5_ENCODER, TIM5_ENCODER_CH1_P10_3, TIM5_ENCODER_CH2_P10_1); //l 为负值
encoder_dir_init(TIM4_ENCODER,TIM4_ENCODER_CH1_P02_8, TIM4_ENCODER_CH2_P00_9);//r
int16 l_encoder_pulse = 0; //左编码器脉冲
int16 r_encoder_pulse = 0; //右编码器脉冲
int16 l_speed_now = 0; //左轮当前速度
int16 r_speed_now = 0; //右轮当前速度
int16 speed_now=0; //整体速度
void get_encoder(void)
{
encoder_l = -encoder_get_count(TIM5_ENCODER);
encoder_clear_count(TIM5_ENCODER); //清除编码器计数// 获取编码器计数
encoder_r = encoder_get_count(TIM4_ENCODER); // 获取编码器计数
encoder_clear_count(TIM4_ENCODER); //清除编码器计数
l_speed_now = encoder_l;
r_speed_now = encoder_r;
// printf("l:%d,%d\n ",encoder_l,encoder_r);
speed_now = (l_speed_now+r_speed_now)/2;
// printf("speed_now:%d\n ",speed_now);
}IPS 2.0寸显示屏
//***@brief: 【初始IPS屏幕】
ips200_set_dir(IPS200_PORTAIT_180);//旋转180度
ips200_init(IPS200_TYPE_PARALLEL8); //并行
ips200_set_color(RGB565_WHITE,RGB565_BLACK);
ips200_full(RGB565_BLACK);
ips200_clear ();IMU963ra陀螺仪
工程
1.头文件#include “filter.h”
#ifndef CODE_filter_H_
#define CODE_filter_H_
#include "headfile.h"
#pragma section all "cpu0_dsram"
float mag_angle_calc();//角度积分
float angle_calc(float mag_angle_m,float gyro_m); //一阶低通互补滤波
void IMU936RA_process(); //控制数据读取进程
int16 Integral1,Integral2; //角度积分
int16 Gyro_value,Mag_value; //角度输出
int16 Angle_list[361]; //地磁计校准
//互补滤波参数
//将本语句与#pragma section all restore语句之间的全局变量都放在CPU0的RAM中
float mag_angle_out; //地磁计校准后的角度
float angle; //数据融合后的角度
float mag_ratio =0.05; //地磁计比例
float gyro_ratio =0.072; //陀螺仪比例
float dt =0.005; //采样周期
#endif /* CODE_filter_H_ */
2.陀螺仪进程
陀螺仪进程最好放进中断中读取
// 初始化九轴模块
imu963ra_init();
void IMU936RA_process()
{
// 获取加速度计数据
imu963ra_get_acc();
// 获取陀螺仪数据
imu963ra_get_gyro();
// 获取磁力计数据
imu963ra_get_mag();
// mag_angle_out= mag_angle_calc(); //地磁计积分
angle = angle_calc(mag_angle_out, imu963ra_gyro_z); //一阶互补滤波计算角度值
// printf("angle=%f\r\n",angle);//左正右负
Kalman_Filter(Mag_value,angle_out); //卡尔曼滤波 Accel 加速度计计算YAW Gyro 陀螺仪的积分， YAW角能通过磁力计算出
printf("angle_dot:%f,%d,%f \r\n",angle_dot,imu963ra_acc_z-4100,angle_out); //卡尔曼输出角度，加速度计，陀螺仪积分
} // 通过串口将采集到的数据发送出去
printf("imu963ra_acc_x = %d\r\n", imu963ra_acc_x);
printf("imu963ra_acc_y = %d\r\n", imu963ra_acc_y); //偏-210
printf("imu963ra_acc_z = %d\r\n", imu963ra_acc_z-4100); //z轴加速度计初始值4100左右 4100
printf("imu963ra_gyro_x = %d\r\n", imu963ra_gyro_x);
printf("imu963ra_gyro_y = %d\r\n", imu963ra_gyro_y);
printf("imu963ra_gyro_z = %d\r\n", imu963ra_gyro_z);
printf("imu963ra_mag_x = %d\r\n", imu963ra_mag_x);
printf("imu963ra_mag_y = %d\r\n", imu963ra_mag_y);
printf("imu963ra_mag_z = %d\r\n", imu963ra_mag_z);
printf("-------------------------\r\n");姿态解算
//后续可以利用地磁计进行校准
int16 Integral1,Integral2;
int16 Gyro_value,Mag_value;
float mag_angle_calc() //地磁角度解算
{
Integral1 += (imu963ra_gyro_z+3);
Gyro_value = Integral1*0.1/14.285*0.005/0.1; //陀螺仪积分得到YAW(偏航)角度
Integral2 += (imu963ra_mag_z+550);
Mag_value = Integral2*0.1/14.285*0.005/0.1; //地磁计积分得到YAW(偏航)角度
return Mag_value;
}滤波
1.一阶互补滤波
//----------------------------------------------------------------
// @brief 一阶互补滤波
// @param mag_angle_m 地磁计数据
// @param gyro_m 陀螺仪数据
// @return float 数据融合后的角度
//----------------------------------------------------------------
//将本语句与#pragma section all restore语句之间的全局变量都放在CPU0的RAM中
float mag_angle_out; //地磁计校准后的角度
float angle; //数据融合后的角度
float angle_out; //陀螺仪积分得到角度
float mag_ratio =0.05; //地磁计比例
float gyro_ratio =0.072; //陀螺仪比例
float dt =0.005; //采样周期
float angle_calc(float mag_angle_m,float gyro_m);
float angle_calc(float mag_angle_m,float gyro_m)
{
float temp_angle;
float gyro_now;
float error_angle;
static float last_angle;
static uint8 first_angle;
if(!first_angle)//判断是否为第一次运行本函数
{
//如果是第一次运行，则将上次角度值设置为与地磁计角度一致
first_angle =1;
last_angle = mag_angle_m;
}
//根据陀螺仪的数据去估算车模距离上次转动了多少角度
gyro_now = gyro_m * gyro_ratio;
//根据测量到的地磁计值转换为角度之后与上次的角度值求偏差
error_angle =(mag_angle_m - last_angle) * mag_ratio;
mag_angle_out = mag_angle_calc();//地磁计解算并校准得到角度
//根据偏差与陀螺仪测量得到的角度值计算当前角度值
temp_angle = last_angle +(error_angle+ gyro_now )* dt;
//保存当前角度值
last_angle = temp_angle;
return temp_angle;
}2.卡尔曼滤波
//变量声明
float K1 =0.02;
float angle, angle_dot; // 角度， 角速度
float Q_angle=0.001; // 陀螺仪噪声的协方差 预测（过程） 过小对模型预测值信任度越高，系统收敛快，反之相反
float Q_gyro=0.003; // 陀螺仪漂移噪声的协方差 预测（过程）
float R_angle=0.5; // 加速度计测量噪声的协方差 测量值 过小出现震荡，过大响应慢
float dt1=0.005; // 积分时间， dt 为滤波器采样时间（秒） 它的值要特别精确
char C_0 = 1; // H 矩阵的一个数
float Q_bias, Angle_err; // Q_bias 为陀螺仪漂移过程噪声协方差
float PCt_0, PCt_1, E; //中间变量
float K_0, K_1, t_0, t_1; //K 是卡尔曼增益， t 是中间变量
float Pdot[4] ={0,0,0,0}; //计算 P 矩阵的中间变量
float PP[2][2] = { { 1, 0 },{ 0, 1 } }; //公式中 P 矩阵， X 的协方差
float aac_angle;
//函数功能：简易卡尔曼滤波
//入口参数：加速度、角速度(陀螺仪·)
//返回 值：无
// X(k|k-1)=A X(k-1|k-1)+B U(k) ………(1)先验估计
// P(k|k-1)=A P(k-1|k-1) A’+Q ………(2)协方差矩阵的预测
// Kg(k)= P(k|k-1) H’ / (HP(k|k-1) H’ + R) ………(3)计算卡尔曼增益
// X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ………(4)通过卡尔曼增益进行修正
// P(k|k)=（I-Kg(k) H） P(k|k-1) ………(5)更新协方差阵
void Kalman_Filter(float Accel,float Gyro) // Accel 加速度计计算角度 Gyro 陀螺仪的积分得到角度，
{
angle+=(Gyro - Q_bias) * dt1; //先验估计 对应第一个公式
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt1; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt1; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt1;
PP[1][1] += Pdot[3] * dt1;
//以上 8 行对应第二个方程
//计算卡尔曼增益 K_0,K_1
Angle_err = Accel - angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0]; //矩阵乘法的中间变量
PCt_1 = C_0 * PP[1][0]; //C_0=1
E = R_angle + C_0 * PCt_0; //分母
K_0 = PCt_0 / E; //卡尔曼增益，两个，一个是 Angle 的，一个是 Q_bias 的
K_1 = PCt_1 / E;
//以上 6 行对应公式 3
//对 PK 进行更新
t_0 = PCt_0; //矩阵计算中间变量，相当于 a
t_1 = C_0 * PP[0][1]; //矩阵计算中间变量，相当于 b
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
//以上 6 行对应公式 5
//通过卡尔曼增益修正当前角度、陀螺仪零点、当前角速度
angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
//对应第 4 个方程
}
3、互补滤波优化
历程
1.陀螺仪+PID控制小车跑直线
uint8 zhixian_pwm_out;
extern float angle; //数据融合后的角度
uint8 Turn(float Angle,float gyro_z);
void zhixian();
void zhixianGO()
{
pwm_duty(ATOM1_CH2_P21_4, 800);
pwm_duty(ATOM0_CH3_P21_5,0);
pwm_duty(ATOM0_CH0_P21_2, 800);
pwm_duty(ATOM0_CH1_P21_3, 0);
zhixian_pwm_out = Turn(angle,imu963ra_gyro_z);
// printf("pwm:%d\n",zhixian_pwm_out);
// printf("angle=%f\n",angle);
if(angle<0)
{
pwm_duty(ATOM1_CH2_P21_4, 500);
pwm_duty(ATOM0_CH3_P21_5,0);
pwm_duty(ATOM0_CH0_P21_2, 500+zhixian_pwm_out);
pwm_duty(ATOM0_CH1_P21_3, 0);
}
else if(angle>0)
{
pwm_duty(ATOM1_CH2_P21_4, 500+zhixian_pwm_out);
pwm_duty(ATOM0_CH3_P21_5,0);
pwm_duty(ATOM0_CH0_P21_2, 500);
pwm_duty(ATOM0_CH1_P21_3, 0);
}
else
{
pwm_duty(ATOM1_CH2_P21_4, 800);
pwm_duty(ATOM0_CH3_P21_5,0);
pwm_duty(ATOM0_CH0_P21_2, 800);
pwm_duty(ATOM0_CH1_P21_3, 0);
}
// printf("LCenter[%d]=%d\r\n",hang,LCenter[hang]);
// printf("x:%d,%d\n",300,zhixianGo_pwm_out);//通过蓝牙发送数据给上位机显示波形
}
/*********************************************************************
转向环：
入口：
**********************************************************************/
uint8 Turn(float Angle,float gyro_z)
{
float Turn_PWM;
float Turn_Kp=18,Turn_Kd=0.005; //speed-300
// float Turn_Kp=30,Turn_Kd=0.9; //speed - 600
float Bias;
// if(Turn_Target>Turn_Amplitude) Turn_Target=Turn_Amplitude; //转速限幅
// if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;
Bias = Angle-0;
//=============turing PD controller==================//
Turn_PWM = Bias*Turn_Kp+gyro_z*Turn_Kd;
return Turn_PWM;
}IMU660RA
ICM_OneOrderFilter();//陀螺仪采集：5ms
//***@brief: 【初始化陀螺仪】
imu660ra_init();#include "imu.h"
float dt = 0.01;
float gyro_x,gyro_y,gyro_z;
float acc_x,acc_y,acc_z;
float Pitch=0,Roll=0,Yaw=0; // Pitch：俯仰角，Roll：横滚角，Yaw：偏航角
float last_gyro_x=0,last_gyro_y=0,last_gyro_z=0;
//变量声明
float K1 =0.02; // 比例增益，用于计算加速度计对姿态角的影响
float angle, angle_dot; // 角度， 角速度
float Q_angle=0.001f; // 陀螺仪噪声的协方差 预测（过程） 过小对模型预测值信任度越高，系统收敛快，反之相反
float Q_gyro=0.003f; // 陀螺仪漂移噪声的协方差 预测（过程）
float R_angle=0.5f; // 加速度计测量噪声的协方差 测量值 过小出现震荡，过大响应慢
float dt1=0.01f; // 积分时间， dt 为滤波器采样时间（秒） 它的值要特别精确
char C_0 = 1; // H 矩阵的一个数
float Q_bias, Angle_err; // Q_bias 为陀螺仪漂移过程噪声协方差
float PCt_0, PCt_1, E; //中间变量
float K_0, K_1, t_0, t_1; //K 是卡尔曼增益， t 是中间变量
float Pdot[4] ={0,0,0,0}; //计算 P 矩阵的中间变量
float PP[2][2] = { { 1, 0 },{ 0, 1 } }; //公式中 P 矩阵， X 的协方差
float aac_angle;
// 根据加速度计读数计算倾角（单位：弧度）
float compute_accel_angle(float accel_x, float accel_y, float accel_z) {
// 计算加速度大小
float accel_mag = sqrt(accel_x*accel_x + accel_y*accel_y + accel_z*accel_z);
// 计算倾角（取反使得加速度向下时角度为0）
float accel_angle = -atan2(accel_y, sqrt(accel_x*accel_x + accel_z*accel_z));
return accel_angle;
}
void ICM_OneOrderFilter(void)
{
// imu660ra_get_acc();
imu660ra_get_gyro();
// gyro_x= imu660ra_gyro_transition(imu660ra_gyro_x); // 单位为°/s
gyro_z= imu660ra_gyro_transition(imu660ra_gyro_z);
acc_x = imu660ra_acc_transition(imu660ra_acc_x); //单位为 g(m/s^2)
acc_y = imu660ra_acc_transition(imu660ra_acc_y); //单位为 g(m/s^2)
acc_z = imu660ra_acc_transition(imu660ra_acc_z); //单位为 g(m/s^2)
float accel_angle =compute_accel_angle(acc_x,acc_y,acc_z);//对加速度计进行处理得到角度
// Yaw+=gyro_z*dt;
// printf("Yaw=%.2f \r\n",Yaw);
// last_gyro_z = gyro_z;
// angle=angle_dot*dt;
Kalman_Filter(accel_angle,gyro_z);
// printf("Yaw=%.2f \r\n",Yaw);
//printf("gyro_x=%.2f, acc_x=%.2f \r\n",gyro_x,acc_x);
}
//函数功能：简易卡尔曼滤波
//入口参数：加速度、角速度(陀螺仪·)
//返回 值：无
// X(k|k-1)=A X(k-1|k-1)+B U(k) ………(1)先验估计
// P(k|k-1)=A P(k-1|k-1) A’+Q ………(2)协方差矩阵的预测
// Kg(k)= P(k|k-1) H’ / (HP(k|k-1) H’ + R) ………(3)计算卡尔曼增益
// X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ………(4)通过卡尔曼增益进行修正
// P(k|k)=（I-Kg(k) H） P(k|k-1) ………(5)更新协方差阵
void Kalman_Filter(float Accel,float Gyro)
{
angle+=(Gyro - Q_bias) * dt1; //先验估计 对应第一个公式 // 根据陀螺仪的数据估计角度
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分 // 更新协方差矩阵P的值
Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt1; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt1; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt1;
PP[1][1] += Pdot[3] * dt1;
//以上 8 行对应第二个方程
//计算卡尔曼增益 K_0,K_1
Angle_err = Accel - angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0]; //矩阵乘法的中间变量
PCt_1 = C_0 * PP[1][0]; //C_0=1
E = R_angle + C_0 * PCt_0; //分母
K_0 = PCt_0 / E; //卡尔曼增益，两个，一个是 Angle 的，一个是 Q_bias 的
K_1 = PCt_1 / E;
//以上 6 行对应公式 3
//对 PK 进行更新
t_0 = PCt_0; //矩阵计算中间变量，相当于 a
t_1 = C_0 * PP[0][1]; //矩阵计算中间变量，相当于 b
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
//以上 6 行对应公式 5
//通过卡尔曼增益修正当前角度、陀螺仪零点、当前角速度
angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
//对应第 4 个方程
}
EEPROM
判断，擦除
#define FLASH_SECTION_INDEX (0) // 存储数据用的扇区
#define FLASH_PAGE_INDEX (11) // 存储数据用的页码 倒数第一个页码
if(flash_check(FLASH_SECTION_INDEX, FLASH_PAGE_INDEX)) // 判断是否有数据
flash_erase_page(FLASH_SECTION_INDEX, FLASH_PAGE_INDEX); // 擦除这一页
printf("\r\n");
flash_read_page_to_buffer(FLASH_SECTION_INDEX, FLASH_PAGE_INDEX); // 如果有数据就将数据从 flash 读取到缓冲区
printf("\r\n FLASH_SECTION_INDEX: %d, FLASH_PAGE_INDEX: %d, origin data is :", FLASH_SECTION_INDEX, FLASH_PAGE_INDEX);
printf("\r\n float_type : %f", flash_union_buffer[0].float_type); // 将缓冲区第 0 个位置的数据以 float 格式输出
printf("\r\n uint32_type: %lu", flash_union_buffer[1].uint32_type); // 将缓冲区第 1 个位置的数据以 uint32 格式输出
printf("\r\n int32_type : %d", flash_union_buffer[2].int32_type); // 将缓冲区第 2 个位置的数据以 int32 格式输出
printf("\r\n uint16_type: %u", flash_union_buffer[3].uint16_type); // 将缓冲区第 3 个位置的数据以 uint16 格式输出
printf("\r\n int16_type : %d", flash_union_buffer[4].int16_type); // 将缓冲区第 4 个位置的数据以 int16 格式输出
printf("\r\n uint8_type : %u", flash_union_buffer[5].uint8_type); // 将缓冲区第 5 个位置的数据以 uint8 格式输出
printf("\r\n int8_type : %d", flash_union_buffer[6].int8_type); // 将缓冲区第 6 个位置的数据以 int8 格式输出写入
写入缓冲区->缓冲区转到flash区
// 请注意 数据缓冲区的每个位置只能存储一种类型的数据
// 请注意 数据缓冲区的每个位置只能存储一种类型的数据
// 请注意 数据缓冲区的每个位置只能存储一种类型的数据
// 例如 flash_data_union_buffer[0] 写入 int8_type 那么只能以 int8_type 读取
// 同样 flash_data_union_buffer[0] 写入 float_type 那么只能以 float_type 读取 flash_union_buffer[0].int8_type = -128; // 向缓冲区第 0 个位置写入
printf("\r\n int8_type : %d", flash_union_buffer[0].int8_type); // 将缓冲区第 0 个位置的数据以 int8 格式输出 得到正确写入数据
flash_union_buffer[0].int8_type -= 1; // 向缓冲区第 0 个位置写入 int8 数据
printf("\r\n int8_type : %d", flash_union_buffer[0].int8_type); // 将缓冲区第 0 个位置的数据以 int8 格式输出 得到正确写入数据
flash_write_page_from_buffer(FLASH_SECTION_INDEX, FLASH_PAGE_INDEX); // 向指定 Flash 扇区的页码写入缓冲区数据
flash_buffer_clear(); // 清空缓冲区
flash_union_buffer[0].float_type = 3.1415926; // 向缓冲区第 0 个位置写入 float 数据
flash_union_buffer[1].uint32_type = 4294967294; // 向缓冲区第 1 个位置写入 uint32 数据
flash_union_buffer[2].int32_type = -2147483648; // 向缓冲区第 2 个位置写入 int32 数据
flash_union_buffer[3].uint16_type = 65535; // 向缓冲区第 3 个位置写入 uint16 数据
flash_union_buffer[4].int16_type = -32768; // 向缓冲区第 4 个位置写入 int16 数据
flash_union_buffer[5].uint8_type = 255; // 向缓冲区第 5 个位置写入 uint8 数据
flash_union_buffer[6].int8_type = -128; // 向缓冲区第 6 个位置写入 int8 数据
printf("\r\n");
flash_buffer_clear(); // 清空缓冲区
printf("\r\n Flash data buffer default data is :");
printf("\r\n float_type : %f", flash_union_buffer[0].float_type); // 将缓冲区第 0 个位置的数据以 float 格式输出
printf("\r\n uint32_type: %lu",flash_union_buffer[1].uint32_type); // 将缓冲区第 1 个位置的数据以 uint32 格式输出
printf("\r\n int32_type : %d", flash_union_buffer[2].int32_type); // 将缓冲区第 2 个位置的数据以 int32 格式输出
printf("\r\n uint16_type: %u", flash_union_buffer[3].uint16_type); // 将缓冲区第 3 个位置的数据以 uint16 格式输出
printf("\r\n int16_type : %d", flash_union_buffer[4].int16_type); // 将缓冲区第 4 个位置的数据以 int16 格式输出
printf("\r\n uint8_type : %u", flash_union_buffer[5].uint8_type); // 将缓冲区第 5 个位置的数据以 uint8 格式输出
printf("\r\n int8_type : %d", flash_union_buffer[6].int8_type); // 将缓冲区第 6 个位置的数据以 int8 格式输出
system_delay_ms(200);读取
flash_read_page_to_buffer(FLASH_SECTION_INDEX, FLASH_PAGE_INDEX); // 将数据从 flash 读取到缓冲区
printf("\r\n FLASH_SECTION_INDEX: %d, FLASH_PAGE_INDEX: %d, new data is :", FLASH_SECTION_INDEX, FLASH_PAGE_INDEX);
printf("\r\n float_type : %f", flash_union_buffer[0].float_type); // 将缓冲区第 0 个位置的数据以 float 格式输出
printf("\r\n uint32_type: %lu",flash_union_buffer[1].uint32_type); // 将缓冲区第 1 个位置的数据以 uint32 格式输出
printf("\r\n int32_type : %d", flash_union_buffer[2].int32_type); // 将缓冲区第 2 个位置的数据以 int32 格式输出
printf("\r\n uint16_type: %u", flash_union_buffer[3].uint16_type); // 将缓冲区第 3 个位置的数据以 uint16 格式输出
printf("\r\n int16_type : %d", flash_union_buffer[4].int16_type); // 将缓冲区第 4 个位置的数据以 int16 格式输出
printf("\r\n uint8_type : %u", flash_union_buffer[5].uint8_type); // 将缓冲区第 5 个位置的数据以 uint8 格式输出
printf("\r\n int8_type : %d", flash_union_buffer[6].int8_type); // 将缓冲区第 6 个位置的数据以 int8 格式输出 MT9V034
//***@brief: 【初始化摄像头】
mt9v03x_init();
ips200_init(IPS200_TYPE_PARALLEL8);//ips2.0初始化 并行
ips200_showstr(0, 0, "SEEKFREE MT9V03x"); //240 320
ips200_showstr(0, 1, "Initializing...");
//如果屏幕一直显示初始化信息，请检查摄像头接线
//如果使用主板，一直卡在while(!uart_receive_flag)，请检查是否电池连接OK?或者摄像头的配置串口与单片机连接是否正确
//如果图像只采集一次，请检查场信号(VSY)是否连接OK?
if(mt9v03x_finish_flag)
{
ips200_displayimage032(Pixle[0], MT9V03X_W, MT9V03X_H);
mt9v03x_finish_flag = 0;
//在图像使用完毕后 务必清除标志位，否则不会开始采集下一幅图像
//注意：一定要在图像使用完毕后在清除此标志位
}舵机
s3010
gtm_pwm_init(ATOM0_CH7_P02_7, 50, 0);//舵机pwm初始化
pwm_duty(ATOM0_CH7_P02_7, 680);//R_max
systick_delay_ms(STM0, 500);
pwm_duty(ATOM0_CH7_P02_7, 750);
systick_delay_ms(STM0, 500);
pwm_duty(ATOM0_CH7_P02_7, 810);//L_max
systick_delay_ms(STM0, 500);
pwm_duty(ATOM0_CH7_P02_7, 750);
systick_delay_ms(STM0, 500);无限透传
配置
//把printf原来串口0改为串口2，通过蓝牙发送数据
//在zf_common_debug.h里面
//#define DEBUG_UART_INDEX (UART_0) // 指定 debug uart 所使用的的串口
//#define DEBUG_UART_BAUDRATE (115200) // 指定 debug uart 所使用的的串口波特率
//#define DEBUG_UART_TX_PIN (UART0_TX_P14_0) // 指定 debug uart 所使用的的串口引脚
//#define DEBUG_UART_RX_PIN (UART0_RX_P14_1) // 指定 debug uart 所使用的的串口引脚
//#define DEBUG_UART_USE_INTERRUPT (0) // 是否启用 debug uart 接收中断
#define DEBUG_UART_INDEX (UART_2) // 指定 debug uart 所使用的的串口
#define DEBUG_UART_BAUDRATE (115200) // 指定 debug uart 所使用的的串口波特率
#define DEBUG_UART_TX_PIN (UART2_RX_P10_6) // 指定 debug uart 所使用的的串口引脚
#define DEBUG_UART_RX_PIN (UART2_TX_P10_5) // 指定 debug uart 所使用的的串口引脚
#define DEBUG_UART_USE_INTERRUPT (0)
使用
//***@brief: 【无线转串口模块相关引脚定义在 wireless.h文件中】
wireless_uart_init();;//无线模块
uart_init(UART_2,115200,UART2_TX_P10_5,UART2_RX_P10_6);//【把printf原来串口0改为串口2，通过蓝牙发送数据 】
//发送给逐飞科技上位机显示图像
uart_putchar(UART_2,0x00);uart_putchar(UART_2,0xff);uart_putchar(UART_2,0x01);uart_putchar(UART_2,0x01);//发送命令
seekfree_wireless_send_buff(Pixle,MT9V03X_H*MT9V03X_W);
//发送给开源上位机
uart_putchar(UART_2,0XFC);uart_putchar(UART_2,0XCF);
seekfree_wireless_send_buff(Pixle,60*94); //无线传输数据到上位机
uart_putchar(UART_2,0XCF);uart_putchar(UART_2,0XFC);//发送命令
uint8 test1[] = "seekfree wireless to uart test\n";
uint8 test2[] = "seekfree.taobao.com\n";
//seekfree_wireless_send_buff(test1,sizeof(test1)-1);//由于sizeof计算字符串的长度包含了最后一个0，因此需要减一
//seekfree_wireless_send_buff(test2,sizeof(test2)-1);
//使用
printf("%d,%d \r\n",test1);
中断
中断要控制的程序放在isr.c里面对应的中断
//中断函数在isr.c中 函数名称为cc60_pit_ch0_isr
//中断相关的配置参数在isr_config.h内
//可配置参数有 CCU6_0_CH0_INT_SERVICE 和 CCU6_0_CH0_ISR_PRIORITY
//CCU6_0_CH0_INT_SERVICE 中断服务者
//CCU6_0_CH0_ISR_PRIORITY 中断优先级 优先级范围1-255 越大优先级越高 与平时使用的单片机不一样
//需要特备注意的是 不可以有优先级相同的中断函数 每个中断的优先级都必须是不一样的 /***@brief: 【初始化周期中断 】***/
pit_ms_init(CCU60_CH0, 5); //5ms 时间周期
pit_ms_init(CCU60_CH1, 50); //50ms 按键扫描
算法
最小二乘法-拟合线
拟合直线函数（y=kx+b）
type:左中右三线
startline:计算的起始行
endline：计算的结束行
parameterB ：斜率
parameterA :截距
计算得到斜率和截距
void Regression(uint8 type, uint8 startline, uint8 endline)
{//一左，二右，0中间
uint8 i = 0;
uint8 sumlines = endline - startline;
int16 sumX = 0;
int16 sumY = 0;
float averageX = 0;
float averageY = 0;
float sumUp = 0;
float sumDown = 0;
if (type == 0) //拟合中线
{
for (i = startline; i < endline; i++)
{
sumX += i;
sumY += m_line_x[i];
}
if (sumlines != 0)
{
averageX = sumX*1.0 / sumlines; //x的平均值
averageY = sumY*1.0 / sumlines; //y的平均值
}
for (i = startline; i < endline; i++)
{
sumUp += (m_line_x[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 1) //拟合左线
{
for (i = startline; i < endline; i++)
{
sumX += i;
sumY += l_line_x[i];
}
averageX = sumX*1.0 / sumlines; //x的平均值
averageY = sumY*1.0 / sumlines; //y的平均值
for (i = startline; i < endline; i++)
{
sumUp += (l_line_x[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 2) //拟合右线
{
for (i = startline; i < endline; i++)
{
sumX += i;
sumY += r_line_x[i];
}
averageX = sumX*1.0 / sumlines; //x的平均值
averageY = sumY*1.0 / sumlines; //y的平均值
for (i = startline; i < endline; i++)
{
sumUp += (r_line_x[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
}画线
void Hua_Xian(uint8 type, uint8 startline, uint8 endline, float parameterB, float parameterA)
{//一左，二右
if (type == 1) //左
{
for (uint8 i = startline; i < endline; i++)
{
l_line_x[i] = (uint8)(parameterB * i + parameterA);
if (l_line_x[i] < 0)
{
l_line_x[i] = 0;
}
}
}
else if (type == 2) //右
{
for (int i = startline; i < endline; i++)
{
r_line_x[i] = (uint8)(parameterB * i + parameterA);
if (r_line_x[i] > 187)
{
r_line_x[i] = 187;
}
}
}
else if (type == 0) //中
{
for (int i = startline; i < endline; i++)
{
m_line_x[i] = (uint8)((l_line_x[i] / 2 + r_line_x[i] / 2));
if (m_line_x[i] < 0)
{
m_line_x[i] = 0;
}
if (m_line_x[i] > 185)
{
m_line_x[i] = 185;
}
}
}
}阈值，图像二值化，，腐蚀，压缩图像
原图像120×188 压缩后60×94
求阈值
1.大津法（OTSU）求阈值
大津法对每张待处理的图片单独计算一个二值化阈值，这种方法较好的解决了白天和晚上光照强度不同而导致阈值变化的问题。但仍然有不足之处。
由于大津法得出二值化阈值后，还是采用的固定阈值二值化的方法，即整张图像采用同一个阈值。这并不能很好的解决反光问题和赛道小范围内光照变化的问题。
由于该处赛道上方有一个光源，导致小范围内的亮度突变。这显然不满足大津法的“双峰”的假设，可想而知大津法得出的结果将无法正常使用。
/************************************
* 函数名称：otsu(uint16 column, uint16 row)
* 功能说明：求阈值大小
* 参数说明：column:MT9V03X_W row:MT9V03X_H
* 函数返回：阈值大小
* 修改时间：2021/5/12 Wed
************************************/
uint16 otsu(uint16 column,uint16 row)
{
uint16 i,j;
uint32 Amount = 0; //计数
uint32 PixleBack = 0; //像素背景
uint32 PixleIntegralBack =0; //像素背景积分
uint32 PixleIntegral = 0; //像素积分
int32 PixleFore = 0; //像素前景
int32 PixleIntegralFore = 0; //像素积分前景
float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; //类间方差
uint16 MinValue, MaxValue;
uint16 Threshold = 0; //阈值
uint8 HistoGram[256]; //（统计学的）直方图，矩形图
/**初始化灰度直方图***/
for(j=0;j<256;j++) HistoGram[j] = 0;
/**统计灰度级中每个像素在整幅图像中的个数化灰度直方图***/
for(j=0;j<row;j++) //行
{
for (i = 0; i < column; i++) //列
{
HistoGram[mt9v03x_image[j][i]]++;
}
}
/*******获取最小，大灰度的值******/
for (MinValue = 0; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++) ; //获取最小灰度的值
for (MaxValue = 255; MaxValue > MinValue && HistoGram[MinValue] == 0; MaxValue--) ; //获取最大灰度的值
/********像素总数*********/
for (j = MinValue; j <= MaxValue; j++) Amount += HistoGram[j];
/********灰度值总数*********/
PixleIntegral=0; //像素积分清零
for (j = MinValue; j <= MaxValue; j++)
{
PixleIntegral += HistoGram[j] * j;
}
SigmaB = -1;
for (j = MinValue; j < MaxValue; j++)
{
PixleBack = PixleBack + HistoGram[j]; //前景像素点数
PixleFore = Amount - PixleBack; //背景像素点数
OmegaBack = (float)PixleBack / Amount; //前景像素百分比
OmegaFore = (float)PixleFore / Amount; //背景像素百分比
PixleIntegralBack += HistoGram[j] * j; //前景灰度值
PixleIntegralFore = PixleIntegral - PixleIntegralBack; //背景灰度值
MicroBack = (float)PixleIntegralBack / PixleBack; //前景灰度百分比
MicroFore = (float)PixleIntegralFore / PixleFore; //背景灰度百分比
Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore); //计算类间方差
if (Sigma > SigmaB) //找出最大类间方差以及对应的阈值
{
SigmaB = Sigma;
Threshold = j;
}
}
return Threshold; //返回最佳阈值;
}
2.优化后的大津法
uint8 Threshold_Deal(uint8* image, uint16 col, uint16 row, uint32 pixel_threshold)
{
#define GrayScale 256
uint16 Image_Width = col;
uint16 Image_Height = row;
int X; uint16 Y;
uint8* data = image;
int HistGram[GrayScale];
for (Y = 0; Y < GrayScale; Y++)
{
HistGram[Y] = 0; //初始化灰度直方图
}
for (Y = 0; Y <Image_Height; Y++) //Y<Image_Height改为Y =Image_Height；以便进行 行二值化
{
//Y=Image_Height;
for (X = 0; X < Image_Width; X++)
{
HistGram[(int)data[Y,X]]++; //统计每个灰度值的个数信息
}
}
uint32 Amount = 0;
uint32 PixelBack = 0;
uint32 PixelIntegralBack = 0;
uint32 PixelIntegral = 0;
int32 PixelIntegralFore = 0;
int32 PixelFore = 0;
double OmegaBack=0, OmegaFore=0, MicroBack=0, MicroFore=0, SigmaB=0, Sigma=0; // 类间方差;
int16 MinValue=0, MaxValue=0;
uint8 Threshold = 0;
for (MinValue = 0; MinValue < 256 && HistGram[MinValue] == 0; MinValue++) ; //获取最小灰度的值
for (MaxValue = 255; MaxValue > MinValue && HistGram[MinValue] == 0; MaxValue--) ; //获取最大灰度的值
if (MaxValue == MinValue)
{
return MaxValue; // 图像中只有一个颜色
}
if (MinValue + 1 == MaxValue)
{
return MinValue; // 图像中只有二个颜色
}
for (Y = MinValue; Y <= MaxValue; Y++)
{
Amount += HistGram[Y]; // 像素总数
}
PixelIntegral = 0;
for (Y = MinValue; Y <= MaxValue; Y++)
{
PixelIntegral += HistGram[Y] * Y;//灰度值总数
}
SigmaB = -1;
for (Y = MinValue; Y < MaxValue; Y++)
{
PixelBack = PixelBack + HistGram[Y]; //前景像素点数
PixelFore = Amount - PixelBack; //背景像素点数
OmegaBack = (double)PixelBack / Amount;//前景像素百分比
OmegaFore = (double)PixelFore / Amount;//背景像素百分比
PixelIntegralBack += HistGram[Y] * Y; //前景灰度值
PixelIntegralFore = PixelIntegral - PixelIntegralBack;//背景灰度值
MicroBack = (double)PixelIntegralBack / PixelBack;//前景灰度百分比
MicroFore = (double)PixelIntegralFore / PixelFore;//背景灰度百分比
Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore);//g
if (Sigma > SigmaB)//遍历最大的类间方差g
{
SigmaB = Sigma;
Threshold = Y;
}
}
return Threshold;
}3.自适应阈值
二值化
1.大津阈值二值化
///************************************
// * @brief 二值化
// * @param mode ： 0：大津法阈值 1：平均阈值
// * @return
// * @note Get_01_Value(0); //使用大津法二值化
// * @date 2021/5/12 WED
// ************************************/
uint8 Image_Use[120][188]; //存放二值化图像
void binaryzation(uint8 mode) //二值化
{
int i = 0,j = 0;
uint32 Threshold;
uint32 tv=0;
if(mode==1)//求平均阈值
{
//累加
for(i = 0; i <MT9V03X_H; i++) //控制行
{
for(j = 0; j <MT9V03X_W; j++) //控制列
{
tv+=mt9v03x_image[i][j]; //累加所有像素点，计算总共的像素点灰度值
}
}
Threshold=tv/MT9V03X_H/MT9V03X_W; //求平均值,光线越暗越小，全黑约35，对着屏幕约160，一般情况下大约100
Threshold=Threshold*7/10+10; //此处阈值设置，根据环境的光线来设定
}
else//大津法
{
Threshold = otsu(MT9V03X_W,MT9V03X_H); //大津法阈值
// Threshold = (uint8)(Threshold * 0.5) + 70;
}
/**********/
// ips200_display_chinese(80,210,16,chinese_test[60],2,RGB565_BLUE);//阈值
// ips200_showuint16(120,13,Threshold);//显示阈值
//从左下角开始为（0,0）
for(i = 0; i < MT9V03X_H-1; i++)//行
{
for(j = 0; j < MT9V03X_W-1; j++)//列
{
if(mt9v03x_image[i][j] >Threshold) //数值越大，显示的内容越多，较浅的图像也能显示出来
Image_Use[i][j] =0Xff;//大于阈值就为白
else
Image_Use[i][j] =0x00;//小于阈值就为黑
}
}
}2.阳光二值化
//二值化
uint8 Threshold; //阈值
uint8 Threshold_static = 80; //阈值静态下限
uint16 Threshold_detach = 256; //阳光算法分割阈值(光强越强,该值越大)
void Get01change_Dajin()
{
//优化后的大津法
Threshold = Threshold_Deal(mt9v03x_image,mt9v03x_image_W, mt9v03x_image_H, Threshold_detach);
if (Threshold < Threshold_static)
{
Threshold = Threshold_static;
}
uint8 thre;
for(uint8 y = 0; y < mt9v03x_image_H; y++)
{
for(uint8 x = 0; x < mt9v03x_image_W; x++)
{
if (x <= 15)
thre = Threshold - 10;
else if (x >= mt9v03x_image_W-15)
thre = Threshold - 10;
else
thre = Threshold;
if (mt9v03x_image[y][x] >thre) //数值越大，显示的内容越多，较浅的图像也能显示出来
Pixle[y][x] = 255; //白
else
Pixle[y][x] = 0; //黑
}
}
}腐蚀
/************************************
* @brief 过滤噪点
* @param
* @return
* @note
* @date 2021/5/13 Thur
************************************/
void Pixle_Filter(void)
{
int nr;//行
int nc;//列
for(nr=1;nr<60-1;nr++)//从第1行开始过滤每一列
{
for(nc=1; nc<94-1; nc=nc+1)//第一列开始
{
if((Image_Use[nr][nc]==0xff)&&(((Image_Use[nr-1][nc]==0xff)&&(Image_Use[nr+1][nc]==0xff))||((Image_Use[nr][nc+1]==0xff)&&(Image_Use[nr][nc-1]==0xff))))//行-白白白 白白
{
Image_Use[nr][nc]=0xff;//白
}
else if((Image_Use[nr][nc]==0x00)&&(((Image_Use[nr-1][nc]==0x00)&&(Image_Use[nr+1][nc]==0x00))||((Image_Use[nr][nc+1]==0x00)&&(Image_Use[nr][nc-1]==0x00))))//行-黑黑黑 黑黑
{
Image_Use[nr][nc]=0x00;//黑
}
}
}
}
腐蚀优化版
// 腐蚀算法（像素滤波，去除偶尔出现的噪点，优化版）
void Pixle_Filter_Plus() {
for (uint8_t height = 1; height < MT9V03X_H-1; height++) {
for (uint8_t width = 1; width < MT9V03X_W-1; width++) {
int count = 0;
count += (image_01[height-1][width] == 255);
count += (image_01[height+1][width] == 255);
count += (image_01[height][width-1] == 255);
count += (image_01[height][width+1] == 255);
if (image_01[height][width] == 0 && count >= 3) {
image_01[height][width] = 255;
} else if (image_01[height][width] == 255 && count < 2) {
image_01[height][width] = 0;
}
}
}
}压缩图像
/*************************************************************************
* 函数名称：void Get_Use_Image (void)
* 功能说明：把摄像头采集到原始图像，缩放到赛道识别所需大小
* 参数说明：无
* 函数返回：无
* 修改时间：2020年10月28日
* 备 注： IMAGEW为原始图像的宽度，神眼为188，OV7725为320
* IMAGEH为原始图像的高度，神眼为120，OV7725为240
*************************************************************************/
uint8 Pixle[60][94]; //存放压缩后图像
void Get_Use_Image(void)
{
short i = 0, j = 0, row = 0, line = 0;
for (i = 0; i < MT9V03X_H; i += 2) //神眼高 120 / 2 = 60，
// for (i = 0; i < IMAGEH; i += 3) //OV7725高 240 / 3 = 80，
{
for (j = 0; j <= MT9V03X_W; j += 2) //神眼宽188 / 2 = 94，
// for (j = 0; j <= IMAGEW; j += 3) //OV7725宽320 / 3 = 106，
{
Pixle[row][line] = Image_Use[i][j]; //如果要上位机显示图像，就得[i][j] ,单独图像处理就[120-i][188-j]
line++;
}
line = 0;
row++;
}
}寻线
基础寻线
1.1从中间往两边寻线
//图像数据
uint8 Image_Use[120][188]; //存放二值化图像
uint8 Pixle[60][94]; //存放压缩后图像
//线
uint16 hang; //控制行
uint16 lie; //列
uint8 ZHONGJIAN[60]; //中线位置
uint8 ZUO[60]; //左线位置
uint8 YOU[60]; //右线位置
//点
uint8 AX,AY; //A点
uint8 BX,BY; //B点
uint8 CX,CY; //C点
uint8 DX,DY; //D点
//标志
uint8 L_Flag[60]; //左线找到标志
uint8 R_Flag[60]; //右线找到标志/****************基础扫线********************/
void xunxian1(void)
{
/*************************图像处理（灵魂）********************/
//获取AB点
AY=59; //获取AB点
BY=59;
/***************先找最底下一行中线********************/
for(lie=46;lie>=1;lie--) //从中间向左找上升沿
{
if(Pixle[AY][lie]==0xff && Pixle[AY][lie-1]==0x00) //找到上升沿
{
AX = lie; //A横坐标（左边）
}
else //未找到
{
AX = 4;
}
}
for(lie=46;lie<93;lie++) //从中间向右找上升沿
{
if(Pixle[BY][lie]==0xff && Pixle[BY][lie+1] == 0x00) //找到上升沿
{
BX = lie; //B横坐标（右边）
}
else
{
BX = 93;
}
}
/*******************************************************/
/*****************迭代C点 (左边)********************/
CY = AY-1; //迭代C点
CX = AX-1;
for(hang=CY;hang>1;hang--) //由近及远
{
for(lie=CX;lie<93;lie++) //由左向右
{
if(Pixle[hang][lie] == 0xff) //找到白点
{
CX = lie+1; //得到上一行C点X位置
break;
}
}
if(Pixle[hang-1][CX] == 0x00) //判断上方还有没有黑点
{
CY = hang; //得到C点Y位置
break;
}
}
DY = BY-1; //迭代D点
DX = BX-1;
for(hang=DY;hang>0;hang--) //由近及远
{
for(lie=DX;lie>0;lie--) //由右向左
{
if(Pixle[hang][lie] == 0xff) //找到白点
{
DX = lie+1; //得到上一行D点X位置
break; //跳出此循环
}
}
if(Pixle[hang-1][DX] == 0x00) //判断上方还有没有黑点
{
DY = hang; //得到C点Y位置
break;//跳出此循环
}
}
/*************************找中线******************************/
ZHONGJIAN[59]=46;
ZUO[59]=0;
YOU[59]=93;
ZHONGJIAN[59] = (AX + BX) /2; //最底下一行中心线位置找到
uint8 L_Flag = 0;
uint8 R_Flag = 0;
for(hang=58;hang>2;hang--) //向上迭代求剩下中心线
{
for(lie=ZHONGJIAN[hang+1];lie>=1;lie--) //从中间向左找上升沿
{
if(lie>=93) lie=3;
if(Pixle[hang][lie]==0xff && Pixle[hang][lie-1] == 0x00) //找到上升沿
{
ZUO[hang] = lie; //左线
// lcd_drawpoint(lie+3,hang,BLUE );
L_Flag = 1;
break;
}
else
{
if(hang>=30&& hang<60&&L_Flag==0)
{
ZUO[hang] = 2; //左线
// lcd_drawpoint(ZUO[hang]+3,hang,BLUE );
}
}
}
for(lie=ZHONGJIAN[hang+1];lie<93;lie++) //从中间向右找上升沿
{
if(lie==0) lie=1;
if(Pixle[hang][lie]==0xff && Pixle[hang][lie+1] == 0x00) //找到上升沿
{
YOU[hang] = lie; //右线
// lcd_drawpoint(lie-3,hang,BLUE );
R_Flag = 1;
break;
}
else
{
if(hang>=40&&hang<60&&R_Flag==0)
{
YOU[hang] = 93; //右线
// lcd_drawpoint(YOU[hang]-3,hang,BLUE );
}
}
}
ZHONGJIAN[hang] = (ZUO[hang] + YOU[hang]) /2; //计算当前行中心点
if (Pixle[hang][ZHONGJIAN[hang]] == 0x00 && Pixle[hang-1][ZHONGJIAN[hang]] == 0x00)
{
break_hangshu = hang;
//last_break_hangshu = break_hangshu;
//也就是说二十行一下是不会break的
if (break_hangshu <= 40) //防止在一开始就break
{
break;
}
}
// lcd_drawpoint(ZHONGJIAN[hang],hang,RED);
}
/*************************************************************************/
// print_str("break行数"); printf_int(break_hangshu);
}1.2 找断行
void find_duanhang()
{
int16 duanhang0=0;
int16 duanhang1=0;
uint8 duanhangju=0;
uint8 zhongold2=0;
/*
断行0的原理:由于摄像头视角，呈现出的赛道应该是近大远小的，如果远
处的行 宽度比近处的大，那么认为出现第一个断行。(大家可以想一下十
字路口的下方 赛道宽度变宽)
*/
for(hang=58;hang>5;hang--) //扫断点0
{
if ((YOU[hang] - ZUO[hang]) - (YOU[hang + 1] - ZUO[hang + 1]) >= 4 && hang<=50)
{
duanhang0 = hang + 1;
duanhangju = (YOU[hang + 1] - ZUO[hang + 1]);
break;
}
}
for (hang = (int)(duanhang0 - 3); hang >10; hang--)//扫断行1
{
/*
计算是否存在断行1
*/
if ((YOU[hang] - ZUO[hang]) < duanhangju-15 && ZUO[hang]>5 && YOU[hang]<90)
{
duanhang1 = hang;
// print_str("在第");print_int(duanhang1);printf_str("行第二次断行");
break;
}
}
// print_str("在第");print_int(duanhang0);print_str("行第一次断行 "); print_str("断行间距=");printf_int(duanhangju);
/*
如果扫描到断行0，那么从断行0开始的图像可能有问题，
所以这里选择固定终止往下扫线，寻找断行1的存在。
(断行1的原理，当宽度再次变小且小于断行0处的宽度，
认为是正确的断行1，大家可以想一下十字路口的上半段)
*/
if (duanhang0 <50)
{
zhongold2 = ZHONGJIAN[duanhang0 + 7];
for (hang = (int)(duanhang0 - 3); hang >5; hang--) //固定中值扫线
{
for (lie = (int)zhongold2; lie <93; lie++) //扫右线
{
if (lie == 0) lie = 2;
if (Pixle[hang][ lie - 1] == 0xff && Pixle[hang][ lie] == 0x00) //白黑
{
YOU[hang] = (uint8)lie;
// lcd_drawpoint(lie-3,hang,BLUE );
break;
}
else
{
YOU[hang] = 90;
// lcd_drawpoint(90,hang,BLUE );
}
} //扫右线
for (lie = (int)zhongold2; lie >0; lie--) //扫左线
{
if (lie >= 92) lie = 92;
if (Pixle[hang][lie] == 0x00 && Pixle[hang][ lie+1] == 0xff)
{
ZUO[hang] = (uint8)lie;
// lcd_drawpoint(lie-3,hang,BLUE );
break;
}
else
{
ZUO[hang] = 2;
// lcd_drawpoint(2,hang,BLUE );
}
} //扫左线
}//固定中值扫线
}// if duanhang0 <50
//***************扫断点1**********************//
for (hang = (int)(duanhang0 - 3); hang >10; hang--)
{
/*
计算是否存在断行1
*/
if ((YOU[hang] - ZUO[hang]) < duanhangju-15 && ZUO[hang]>=2 && YOU[hang]<=92)
{
duanhang1 = (int)hang;
// print_str("在第");print_int(duanhang1);printf_str("行第二次断行");
break;
}
}
if (duanhang0 <50 && duanhang1 != 0)
{
/*
找到断行1，开始进入二次扫线模式
二次扫线思路:从断行1处开始使用断行0的中值进行扫线，动态继承中值往下扫。
*/
zhongold2 = ZHONGJIAN[duanhang0 + 7];
int gudingtime = 1;
for (hang = (int)(duanhang1); hang > 10; hang--) //二次扫线
{
for (lie = (int)zhongold2; lie <= 93; lie++) //扫右线
{
if (lie == 0) lie = 2;
if (Pixle[hang][ lie - 1] == 0xff && Pixle[hang][ lie] == 0x00 ) //白
{
YOU[hang] = (uint8)lie;
// lcd_drawpoint(lie-3,hang,BLUE );
break;
}
else
{
YOU[hang] = 90;
// lcd_drawpoint(90,hang,BLUE );
}
}
for (lie = (int)zhongold2; lie >1; lie--) //扫左线
{
if (lie >= 93) lie = 93;
if (Pixle[hang][ lie + 1] == 0xff && Pixle[hang][ lie] == 0x00 )
{
ZUO[hang] = (uint8)lie;
// lcd_drawpoint(lie-3,hang,BLUE );
break;
}
else
{
ZUO[hang] = 2;
// lcd_drawpoint(2,hang,BLUE );
}
}
if (gudingtime != 0) gudingtime = gudingtime + 1;
if (gudingtime == 8) gudingtime = 0;
if (gudingtime != 0) zhongold2 = ZHONGJIAN[duanhang0 + 7];
if (gudingtime == 0) zhongold2 = (ZUO[hang] + YOU[hang]) / 2;
}//二次扫线
} //if (duanhang0 <50 && duanhang1 != 0)
}1.3 扫断点
void duanxian_judgment()
{
//判断左线断裂行
L_begin_flag = 0;
for(hang=50;hang>10;hang--)
{
if (Abs(ZUO[hang] - ZUO[hang - 1]) >= 7)//扫到断行
{
leftline_duan_dian = hang; //记录断行开始行
L_begin_flag = 1; //标志断行开始
break;
}
}
if(L_begin_flag==1)//发现断行
{
for(hang=leftline_duan_dian-1;hang>10;hang--) //在断行往后扫
{
if (L_Flag[hang]==1) //扫到断行结束
{
leftline_duan_dian1 = hang; //断线结束行
L_end_flag = 1; //标志找到断线结束行
break;
}
else L_end_flag=0; //标志未找到断线结束行
}
}
//判断右线断裂行
R_begin_flag = 0;
for(hang=50;hang>10;hang--)
{
if (Abs(YOU[hang] - YOU[hang - 1]) >= 7)//扫到断行
{
rightline_duan_dian = hang; //记录断行开始行
R_begin_flag = 1; //标志断行开始
break;
}
}
if(R_begin_flag==1)//发现断行
{
for(hang=rightline_duan_dian-1;hang>10;hang--) //在断行往后扫
{
if (R_Flag[hang]==1) //扫到断行结束
{
rightline_duan_dian1 = hang;
R_end_flag = 1; //标志找到断线结束行
break;
}
else R_end_flag = 0;
}
}
// print_str("左线断裂开始：");print_int(leftline_duan_dian); print_str("行 "); print_str("左线断裂结束：");print_int(leftline_duan_dian1); printf_str("行");
// print_str("右线断裂开始：");print_int(rightline_duan_dian); print_str("行 "); print_str("右线断裂结束：");print_int(rightline_duan_dian1); printf_str("行");
//*********************拟合左右断线
if ( R_begin_flag==1 && R_end_flag==1) //发现右线断裂
{
advanced_regression(2, rightline_duan_dian+2, rightline_duan_dian +3, rightline_duan_dian1-4, rightline_duan_dian1-1); //最小二乘法拟合,得到斜率截距
monirightfuzhi(parameterA, parameterB, rightline_duan_dian, rightline_duan_dian1); //输出模拟线moniright[]
rightlinefuzhi(parameterA, parameterB, rightline_duan_dian-1, rightline_duan_dian1 +1 ); //赋值右线
// print_str("monirightR:");printf_int(moniright[30]);
// print_str("YOU:");printf_int(YOU[38]);
for(uint8 i=rightline_duan_dian;i>rightline_duan_dian1;i--)
{
// lcd_drawpoint(YOU[i]-3,i,BROWN );
}
}
if (L_begin_flag==1&&L_end_flag==1) //发现左线断裂
{
advanced_regression(1, leftline_duan_dian+2, leftline_duan_dian +3, leftline_duan_dian1-4, leftline_duan_dian1-1); //最小二乘法拟合,得到斜率截距
monirightfuzhi(parameterA, parameterB, leftline_duan_dian, leftline_duan_dian1); //输出模拟线moniright[]
rightlinefuzhi(parameterA, parameterB, leftline_duan_dian-1, leftline_duan_dian1 +1 ); //赋值右线
// print_str("monirightL:");printf_int(monileft[30]);
// print_str("ZUO:");printf_int(ZUO[38]);
for(uint8 i=leftline_duan_dian;i>leftline_duan_dian1;i--)
{
// lcd_drawpoint(ZUO[i]-3,i,BLUE );
}
}
/***********找断点******************************************************/
for(hang=59;hang>20;hang--)
{
ZHONGJIAN[hang] = (ZUO[hang] + YOU[hang]) /2; //计算当前行中心点
// lcd_drawpoint(ZHONGJIAN[hang],hang,RED);
}
/*************************************************************************/
//print_str("break行数"); printf_int(break_hangshu);
}八邻域扫线
/************************************************************
【函数名称】Eight_neighborhood
【功 能】八邻域算法求边界
【参 数】寻种子方式
【返 回 值】无
【实 例】Eight_neighborhood(0);
【注意事项】
1*该算法需要你传入的二值化数组，是经过二值化之后的数组，白色为1 黑色为0
2*该算法输出的图像需要你自己定义一个与二值化数组尺寸相同的二维数组
3*下面的宏定义必须换成自己实际的图像数组尺寸和二维数组名
4*记得声明该函数
************************************************************/
//这里是数据类型别名，需要的开启，或者改成你自己的数据类型 如: u8 uint8_t unsigned char
//typedef unsigned char uint8;
//typedef char int8
//typedef unsigned int uint32;
//这里换上你的图像尺寸
//#define USER_SIZE_H 60
//#define USER_SIZE_W 90
//这里换上你二值化之后的图像数组
//#define User_Image image
//这里是八邻域输出数组需要自己定义一个与原图像相同尺寸的二维数组
//#define Edge_arr image
void Transfer_Camera()
{
for(uint8 y=0; y<60; y++) //存储到一个新数组，后续处理（High为120，Width为188，刷新率为50）
{
for(uint8 x=0; x<94; x++)
{
User_Image[y][x] = Pixle[y][x];
}
}
}
int USER_SIZE_H=59;
int USER_SIZE_W=93;
int Edge_arr[60][94];
int User_Image[60][94];
void Eight_neighborhood(uint8 flag)
{
uint8 i,j;
//核子中心坐标 起始方向初始为6
uint8 core_x=0,core_y=0,start=0;
//方向
int8 direction;
uint32 length = 0;
//清空边界数据
for(i=0;i<USER_SIZE_H;i++)
for(j=0;j<USER_SIZE_W;j++)
Edge_arr[i][j]=0;
if(flag==0)//从里向外找种子
{
printf_str("flag=0");
start = 6;
//如果中间为白
print_str("User_Image[USER_SIZE_H-1][USER_SIZE_W/2]="); printf_int(User_Image[USER_SIZE_H-1][USER_SIZE_W/2]);
if(User_Image[USER_SIZE_H-1][USER_SIZE_W/2]==0xff)
{
for(i=USER_SIZE_W/2;i>=0;i--)
{
if(User_Image[USER_SIZE_H-1][i-1]==0x00||i==0)
{//将左下第一个边界点作为种子
printf_str("2");
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
}//如果中间为黑则去两边找
else if(User_Image[USER_SIZE_H-1][USER_SIZE_W/2]==0x00)
{
printf_str("中间为黑则去两边找");
print_str("User_Image[USER_SIZE_H-1][5]="); printf_int(User_Image[USER_SIZE_H-1][5]);
print_str("User_Image[USER_SIZE_H-1][USER_SIZE_W-5="); printf_int(User_Image[USER_SIZE_H-1][USER_SIZE_W-5]);
if(User_Image[USER_SIZE_H-1][5]==0xff)
for(i=5;i>=0;i--)
{
if(User_Image[USER_SIZE_H-1][i-1]==0x00||i==0)
{//将左下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
else if(User_Image[USER_SIZE_H-1][USER_SIZE_W-5]==0xff)
for(i=USER_SIZE_W-5;i>=0;i--)
{
if(User_Image[USER_SIZE_H-1][i-1]==0x00||i==0)
{//将左下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
else//否则将视为无效图像不做处理
return;
}
}
else if(flag==1)
{
start = 6;
for(i=0;i<USER_SIZE_W;i++)
{
if(User_Image[USER_SIZE_H-1][i]==0xff||i==USER_SIZE_W-1)
{//将左下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
}
else if(flag==2)
{
start = 2;
//如果中间为白
if(User_Image[USER_SIZE_H-1][USER_SIZE_W/2]==0xff)
{
for(i=USER_SIZE_W/2;i<USER_SIZE_W;i++)
{
if(User_Image[USER_SIZE_H-1][i+1]==0x00||i==USER_SIZE_W-1)
{
//将右下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
}//如果中间为黑则去两边找
else if(User_Image[USER_SIZE_H-1][USER_SIZE_W/2]==0x00)
{
if(User_Image[USER_SIZE_H-1][5]==0xff)
for(i=5;i<USER_SIZE_W;i++)
{
if(User_Image[USER_SIZE_H-1][i+1]==0x00||i==USER_SIZE_W-1)
{//将右下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
else if(User_Image[USER_SIZE_H-1][USER_SIZE_W-5]==0xff)
for(i=USER_SIZE_W-5;i<USER_SIZE_W;i++)
{
if(User_Image[USER_SIZE_H-1][i+1]==0x00||i==USER_SIZE_W-1)
{//将右下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
else//否则将视为无效图像不做处理
return;
}
}
else if(flag==3)
{
start = 2;
for(i=USER_SIZE_W-1;i>=0;i--)
{
if(User_Image[USER_SIZE_H-1][i]==0xff||i==0)
{//将右下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
}
byte x=0;
put_int(x, x);
//寻找边缘
while(true)
{
direction = start;
Edge_arr[core_y][core_x]=0xff;
if(flag == 0||flag == 1)
{
for(i=0;i<8;i++)
{
if(direction == 0) {if(core_y!=0) {if(User_Image[core_y-1][core_x]==0xff) {core_y--; start=6; break;}}}
if(direction == 1) {if(core_y!=0&&core_x!=USER_SIZE_W-1) {if(User_Image[core_y-1][core_x+1]==0xff) {core_y--; core_x++; start=6; break;}}}
if(direction == 2) {if(core_x!=USER_SIZE_W-1) {if(User_Image[core_y][core_x+1]==0xff) {core_x++; start=0; break;}}}
if(direction == 3) {if(core_y!=USER_SIZE_H-1&&core_x!=USER_SIZE_W-1) {if(User_Image[core_y+1][core_x+1]==0xff) {core_y++; core_x++; start=0; break;}}}
if(direction == 4) {if(core_y!=USER_SIZE_H-1) {if(User_Image[core_y+1][core_x]==0xff) {core_y++; start=2; break;}}}
if(direction == 5) {if(core_y!=USER_SIZE_H-1&&core_x!=0) {if(User_Image[core_y+1][core_x-1]==0xff) {core_y++; core_x--; start=2; break;}}}
if(direction == 6) {if(core_x!=0) {if(User_Image[core_y][core_x-1]==0xff) {core_x--; start=4; break;}}}
if(direction == 7) {if(core_y!=0&&core_x!=0) {if(User_Image[core_y-1][core_x-1]==0xff) {core_y--; core_x--; start=4; break;}}}
direction++; if(direction>7) direction=0;
}
}
else if(flag == 2||flag == 3)
{
for(i=0;i<8;i++)
{
if(direction == 0) {if(core_y!=0) {if(User_Image[core_y-1][core_x]==0xff) {core_y--; start=2; break;}}}
if(direction == 1) {if(core_y!=0&&core_x!=USER_SIZE_W-1) {if(User_Image[core_y-1][core_x+1]==0xff) {core_y--; core_x++; start=4; break;}}}
if(direction == 2) {if(core_x!=USER_SIZE_W-1) {if(User_Image[core_y][core_x+1]==0xff) {core_x++; start=4; break;}}}
if(direction == 3) {if(core_y!=USER_SIZE_H-1&&core_x!=USER_SIZE_W-1) {if(User_Image[core_y+1][core_x+1]==0xff) {core_y++; core_x++; start=6; break;}}}
if(direction == 4) {if(core_y!=USER_SIZE_H-1) {if(User_Image[core_y+1][core_x]==0xff) {core_y++; start=6; break;}}}
if(direction == 5) {if(core_y!=USER_SIZE_H-1&&core_x!=0) {if(User_Image[core_y+1][core_x-1]==0xff) {core_y++; core_x--; start=0; break;}}}
if(direction == 6) {if(core_x!=0) {if(User_Image[core_y][core_x-1]==0xff) {core_x--; start=0; break;}}}
if(direction == 7) {if(core_y!=0&&core_x!=0) {if(User_Image[core_y-1][core_x-1]==0xff) {core_y--; core_x--; start=2; break;}}}
direction--; if(direction==-1) direction=7;
}
}
if(core_y==USER_SIZE_H-1&&length>5)
{
Edge_arr[core_y][core_x]=1;
break;
}
length++;
}
if(flag==0&&length<80)
{
Eight_neighborhood(1);
}
if(flag==1&&length<80)
{
Eight_neighborhood(2);
}
if(flag==2&&length<80)
{
Eight_neighborhood(3);
}
}优化版八邻域
/************************************************************
【函数名称】Eight_neighborhood
【功 能】八邻域算法求边界
【参 数】寻种子方式
【返 回 值】无
【实 例】Eight_neighborhood(0);
【注意事项】
1*该算法需要你传入的二值化数组，是经过二值化之后的数组，白色为1 黑色为0
2*该算法输出的图像需要你自己定义一个与二值化数组尺寸相同的二维数组
3*下面的宏定义必须换成自己实际的图像数组尺寸和二维数组名
4*记得声明该函数
************************************************************/
//这里是数据类型别名，需要的开启，或者改成你自己的数据类型 如: u8 uint8_t _t unsigned char
//typedef unsigned char uint8_t ;
//typedef char int8
//typedef unsigned int uint32;
//这里换上你的图像尺寸
#define USER_SIZE_H 120
#define USER_SIZE_W 188
int Edge_arr[120][188];
//这里换上你二值化之后的图像数组
#define User_Image image_01
//这里是八邻域输出数组需要自己定义一个与原图像相同尺寸的二维数组
#define Edge_arr image_8
uint8_t L_direction_state=0; // 种子方向状态 0-直线,,1-横左,2-竖线,4-横右
uint8_t L_direction_state_l=0; //上次状态
uint8_t L_direction_state_ll=0;
uint8_t L_direction_state_lll=0;
uint8_t L_inflection_corss_point_flag=0; //左十字拐点标志位
uint8_t L_inflection_corss_core_y1,L_inflection_corss_core_x1; //十字左下拐点坐标
uint8_t L_inflection_corss_core_y2,L_inflection_corss_core_x2; //十字左上拐点坐标
uint8_t L_inflection_LeftCirque_point_flag=0; //左圆环拐点标志位
uint8_t L_inflection_LeftCirque_core_y1,L_inflection_LeftCirque_core_x1; //左圆环左下拐点坐标
uint8_t L_inflection_LeftCirque_core_y2,L_inflection_LeftCirque_core_x2; //左圆环左上拐点坐标
uint8_t R_direction_state=0; // 0-直线,,1-横左,2-竖线,4-横右
uint8_t R_direction_state_l=0;
uint8_t R_direction_state_ll=0;
uint8_t R_inflection_corss_point_flag=0; //右十字拐点标志位
uint8_t R_inflection_corss_core_y1,R_inflection_corss_core_x1; //右下拐点坐标
uint8_t R_inflection_corss_core_y2,R_inflection_corss_core_x2; //右上拐点坐标
//uint8_t L_inflection_flag1=0,R_inflection_flag1=0; //拐点状态识别
//uint8_t L_inflection_flag2=0,R_inflection_flag2=0; //拐点状态识别
//uint8_t L_inflection_flag3=0,R_inflection_flag3=0; //拐点状态识别
//uint8_t L_inflection_flag4=0,R_inflection_flag4=0; //拐点状态识别
//uint8_t L_inflection_flag5=0,R_inflection_flag5=0; //拐点状态识别
//uint8_t L_inflection_flag6=0,R_inflection_flag6=0; //拐点状态识别
uint8_t zhongzi_direction_point; //种子方向点
uint8_t zhongzi_direction_arr[3]={0,0,0}; //种子方向数组
uint8_t zhongzi_direction; //种子方向
uint8_t L_straight_point_num; //左边线直线点种子数量
uint8_t L_su_num; //左边线竖直线种子
uint8_t L_ternleft_point_num; //左边向左横向生长种子数量
uint8_t L_ternright_point_num; //左边向右横向生长种子数量
uint8_t L_Lower_left_point_num; //左边左下生长种子数量
uint8_t R_straight_point_num; //右边线直线点种子数量
uint8_t R_su_num; //右边线竖直线种子
uint8_t R_ternright_point_num; //右边向右横向生长种子数量
uint8_t R_ternleft_point_num; //右边向左横向生长种子数量
uint8_t R_up_right_point_num; //右边左上生长种子数量
uint8_t core_x_max=0,core_xdey=0;
uint8_t core_y_max=0,core_ydex=0;
uint8_t core_x_min=188;
uint8_t huandao_stage=0; //环岛阶段标志位
//赛道标志
uint8_t straight,bend,Cross,L_Cross,LeftCirque;
uint8_t cross_left[2] ={0, 0}; //第一个下标存图像下方的点的y值
uint8_t cross_right[2] ={0, 0};
uint8_t core_x,core_y,start;
//方向
int8 direction;
uint8_t L_zhixian=0,L_zuo=0,L_you=0,L_su=0;//方向权重
uint8_t a=0;
void Eight_neighborhood(uint8_t flag)
{
uint8_t i,j;
//核子中心坐标 起始方向初始为6
uint8_t core_x=0,core_y=0,start=0;
//方向
int8 direction;
uint32 lengths = 0;
//清空边界数据
for(i=0;i<USER_SIZE_H;i++)
for(j=0;j<USER_SIZE_W;j++)
Edge_arr[i][j]=0;
// 根据 flag 的值判断从里向外找种子点还是从外向里找种子点，这里是从里向外找
if(flag==0)//从里向外找种子
{
start = 6;
//如果中间为白
if(User_Image[USER_SIZE_H-1][USER_SIZE_W/2]==0xff)
{
for(i=USER_SIZE_W/2;i>=0;i--)
{
if(User_Image[USER_SIZE_H-1][i-1]==0x00||i==0)
{//将左下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
}
//如果中间为黑则去两边找种子
else if(User_Image[USER_SIZE_H-1][USER_SIZE_W/2]==0x00)
{
if(User_Image[USER_SIZE_H-1][5]==0xff)
for(i=5;i>=0;i--)
{
if(User_Image[USER_SIZE_H-1][i-1]==0x00||i==0)
{//将左下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
else if(User_Image[USER_SIZE_H-1][USER_SIZE_W-5]==0xff)
for(i=USER_SIZE_W-5;i>=0;i--)
{
if(User_Image[USER_SIZE_H-1][i-1]==0x00||i==0)
{//将左下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
else//否则将视为无效图像不做处理
return;
}
}
else if(flag==1)
{
start = 6;
for(i=0;i<USER_SIZE_W;i++)
{
if(User_Image[USER_SIZE_H-1][i]==0xff||i==USER_SIZE_W-1)
{//将左下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
}
else if(flag==2)
{
start = 2;
//如果中间为白
if(User_Image[USER_SIZE_H-1][USER_SIZE_W/2]==0xff)
{
for(i=USER_SIZE_W/2;i<USER_SIZE_W;i++)
{
if(User_Image[USER_SIZE_H-1][i+1]==0x00||i==USER_SIZE_W-1)
{
//将右下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
}//如果中间为黑则去两边找
else if(User_Image[USER_SIZE_H-1][USER_SIZE_W/2]==0x00)
{
if(User_Image[USER_SIZE_H-1][5]==0xff)
for(i=5;i<USER_SIZE_W;i++)
{
if(User_Image[USER_SIZE_H-1][i+1]==0x00||i==USER_SIZE_W-1)
{//将右下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
else if(User_Image[USER_SIZE_H-1][USER_SIZE_W-5]==0xff)
for(i=USER_SIZE_W-5;i<USER_SIZE_W;i++)
{
if(User_Image[USER_SIZE_H-1][i+1]==0x00||i==USER_SIZE_W-1)
{//将右下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
else//否则将视为无效图像不做处理
return;
}
}
else if(flag==3)
{
start = 2;
for(i=USER_SIZE_W-1;i>=0;i--)
{
if(User_Image[USER_SIZE_H-1][i]==0xff||i==0)
{//将右下第一个边界点作为种子
core_x = i;
core_y = USER_SIZE_H-1;
break;
}
}
}
//寻找边缘
while(true)
{
direction = start;
Edge_arr[core_y][core_x]=0xff;
/*******左边线种子********************************************/
if(flag == 0||flag == 1)
{
//x--;
// print_str("x=");print_int(x);
//if(x<10) {x=119;break;}
// for(i=0;i<8;i++)
// {
// //print_str(" direction=");print_int(direction);
// if(direction == 0 && core_y!=0) { if(image_01[core_y-1][core_x]==0xff) {core_y--; start=6; break;}} //print_str(" start="); print_int(start);
// if(direction == 1) {if(core_y!=0&&core_x!=USER_SIZE_W-1) {if(image_01[core_y-1][core_x+1]==0xff) {core_y--; core_x++; start=6; break;}}}
// if(direction == 2) {if(core_x!=USER_SIZE_W-1) {if(image_01[core_y][core_x+1]==0xff) {core_x++; start=0; break;}}}
// if(direction == 3) {if(core_y!=USER_SIZE_H-1&&core_x!=USER_SIZE_W-1) {if(image_01[core_y+1][core_x+1]==0xff) {core_y++; core_x++; start=0; break;}}}
// if(direction == 4) {if(core_y!=USER_SIZE_H-1) {if(image_01[core_y+1][core_x]==0xff) {core_y++; start=2; break;}}}
// if(direction == 5) {if(core_y!=USER_SIZE_H-1&&core_x!=0) {if(image_01[core_y+1][core_x-1]==0xff) {core_y++; core_x--; start=2; break;}}}
// if(direction == 6) {if(core_x!=0) {if(image_01[core_y][core_x-1]==0xff) {core_x--; start=4; break;}}}
// if(direction == 7) {if(core_y!=0&&core_x!=0) {if(image_01[core_y-1][core_x-1]==0xff) {core_y--; core_x--; start=4; break;}}}
// direction++; if(direction>7) direction=0;
// }
// find_zhongzi(); //寻找种子方向，与个数统计，拐点寻找
//优化后此代码使用两个数组dx和dy来存储八个可能方向中每个方向的x和y偏移。然后，它在每个方向上循环，并根据当前方向以及dx和dy中的偏移量计算新的x和y坐标。如果新坐标在图像的边界内，并且该位置的像素具有目标值，它会更新core_x和core_y变量，并将起始变量设置为适当的值。
for (int i = 0; i < NUM_DIRECTIONS; i++) {
int dir = (direction + i) % NUM_DIRECTIONS;
int new_x = core_x + dx[dir];
int new_y = core_y + dy[dir];
if (new_x >= 0 && new_x < USER_SIZE_W && new_y >= 0 && new_y < USER_SIZE_H && image_01[new_y][new_x] == TARGET_VALUE) {
core_x = new_x;
core_y = new_y;
start = (dir + 6) % NUM_DIRECTIONS;
break;
}
}
}
else if(flag == 2||flag == 3)
{
for(i=0;i<8;i++)
{
if(direction == 0) {if(core_y!=0) {if(User_Image[core_y-1][core_x]==0xff) {core_y--; start=2; break;}}}
if(direction == 1) {if(core_y!=0&&core_x!=USER_SIZE_W-1) {if(User_Image[core_y-1][core_x+1]==0xff) {core_y--; core_x++; start=4; break;}}}
if(direction == 2) {if(core_x!=USER_SIZE_W-1) {if(User_Image[core_y][core_x+1]==0xff) {core_x++; start=4; break;}}}
if(direction == 3) {if(core_y!=USER_SIZE_H-1&&core_x!=USER_SIZE_W-1) {if(User_Image[core_y+1][core_x+1]==0xff) {core_y++; core_x++; start=6; break;}}}
if(direction == 4) {if(core_y!=USER_SIZE_H-1) {if(User_Image[core_y+1][core_x]==0xff) {core_y++; start=6; break;}}}
if(direction == 5) {if(core_y!=USER_SIZE_H-1&&core_x!=0) {if(User_Image[core_y+1][core_x-1]==0xff) {core_y++; core_x--; start=0; break;}}}
if(direction == 6) {if(core_x!=0) {if(User_Image[core_y][core_x-1]==0xff) {core_x--; start=0; break;}}}
if(direction == 7) {if(core_y!=0&&core_x!=0) {if(User_Image[core_y-1][core_x-1]==0xff) {core_y--; core_x--; start=2; break;}}}
direction--; if(direction==-1) direction=7;
}
}
//左边界扫完就跳出while循环
if(core_y==USER_SIZE_H-1&&lengths>5)
{
Edge_arr[core_y][core_x]=1; //八邻域数组赋值
break;
}
lengths++;
}//while
if(flag==0&&lengths<80)
{
Eight_neighborhood(1);
}
if(flag==1&&lengths<80)
{
Eight_neighborhood(2);
}
if(flag==2&&lengths<80)
{
Eight_neighborhood(3);
}
}2.2.根据八邻域后寻线
int pixel1;
int pixel2;
int pixel3;
int found_line = 0;
void Search_Line(uint8_t type) //0为普通模式，1为斑马线模式（只有搜线起始横坐标不同）
{
uint8_t l_flag=0,r_flag=0;
uint8_t l_search_start, r_search_start; //搜线起始列坐标
uint8_t r_searchget_flage, l_searchget_flage; //搜到线时的标志位
r_searchget_flage=1; l_searchget_flage=1; //开始搜索是默认为上次搜到线
r_lose_value=0; l_lose_value=0; //左右丢线数，后面注意要走一次清零 //左右丢线数，后面注意要走一次清零
// AY = 119; //获取AB点
// BY = 119;
// /***************先找最底下一行中线********************/
// for(y=93;y>=1;y--) //从中间向左找上升沿
// {
// if(image_8[AY][y]==0xff && image_8[AY][y-1]==0x00) //找到上升沿
// {
// AX = y; //A横坐标（左边）
// }
// else //未找到
// {
// AX = 4;
//
// }
//
// }
// for(y=93;y<187;y++) //从中间向右找上升沿
// {
// if(image_8[BY][y]==0xff && image_8[BY][y+1] == 0x00) //找到上升沿
// {
// BX = y; //B横坐标（右边）
// }
// else
// {
//
// BX = 187;
// }
// }
// /*****************迭代C点 (左边)********************/
// CY = AY-1; //迭代C点
// CX = AX-1;
//
// for(height=CY;height>1;height--) //由近及远
// {
// for(y=CX;y<187;y++) //由左向右
// {
// if(image_8[height][y] == 0xff) //找到白点
// {
//
// CX = y+1; //得到上一行C点X位置
// break;
// }
// }
// if(image_8[height-1][CX] == 0x00) //判断上方还有没有黑点
// {
// CY = height; //得到C点Y位置
// break;
// }
//
// }
//
// DY = BY-1; //迭代D点
// DX = BX-1;
// for(height=DY;height>0;height--) //由近及远
// {
// for(y=DX;y>0;y--) //由右向左
// {
// if(image_8[height][y] == 0xff) //找到白点
// {
// DX = y+1; //得到上一行D点X位置
// break; //跳出此循环
// }
// }
// if(image_8[height-1][DX] == 0x00) //判断上方还有没有黑点
// {
// DY = height; //得到C点Y位置
// break;//跳出此循环
// }
// }
// m_line_x[119] = (AX + BX) /2; //最底下一行中心线位置找到
for(height=(MT9V03X_H-1);height>search_line_end && height>=10;height--)
{
//确定每行的搜线起始横坐标
if (type == 0) {//普通模式
if (height >115 || (l_line_x[height + 1] == 0 && r_line_x[height + 1] == 187 && height < 116)) {
// Both lines are lost
l_search_start = r_search_start = MT9V03X_W / 2;
} else if (l_line_x[height + 1] != 0 && r_line_x[height + 1] != 187 && height < 116) {
// Both lines are detected
l_search_start = l_line_x[height + 1] + 7;
r_search_start = r_line_x[height + 1] - 7;
} else if (l_line_x[height + 1] != 0 && height < 116) {
// Only left line is detected
l_search_start = l_line_x[height + 1] + 7;
r_search_start = MIN(187, l_search_start + 100);
} else if (r_line_x[height + 1] != 187 && height < 116) {
// Only right line is detected
r_search_start = r_line_x[height + 1] - 7;
l_search_start = MAX(0, r_search_start - 100);//避免负值
}
}
if( (image_01[height][MT9V03X_W/2] ==0x00) && (image_01[height -1][MT9V03X_W/2] ==0x00) && (image_01[height -2][MT9V03X_W/2] ==0x00) && (height <MT9V03X_H-30)) //搜线终止条件
{
search_line_end = height+1;
break;
}
else
{
search_line_end = 20;
}
// 八领域扫线边线为白，其他为黑色
// for(l_width=l_search_start; l_width>1; l_width--) //左边搜线
// {
// if(image_8[height][l_width -2]==0xff && image_8[height][l_width -1]==0x00 && image_8[height][l_width] ==0x00 && l_width>2)
// { //黑黑白
// l_line_x[height] = l_width-2; //记录边线列
// l_line_y[height] = height; //记录边线行
// l_search_flag[height] = 1; //标记该行找到边线
// l_searchget_flage = 1; //左边线找到
// break;
// }
// else if(l_width==2)
// {
// if(l_flag==0)
// {
// l_flag=1;
// l_losemax=height;
// }
// l_line_x[height] = 0;
// l_line_y[height] = height;
// l_search_flag[height] = 0; //改行没有搜到线
// l_searchget_flage = 0; //左边界没有搜到线
// l_lose_value++; //丢线次数
// break;
// }
// }
//2
l_width = l_search_start;
pixel1 = image_8[height][l_width - 2];
pixel2 = image_8[height][l_width - 1];
pixel3 = image_8[height][l_width];
while (l_width > 1 && !(pixel1 == 0xff && pixel2 == 0x00 && pixel3 == 0x00)) {
l_width--;
pixel1 = pixel2;
pixel2 = pixel3;
pixel3 = image_8[height][l_width];
}
if (l_width > 1) {
l_line_x[height] = l_width - 2;
l_line_y[height] = height;
l_search_flag[height] = 1;
l_searchget_flage = 1;
} else {
if (l_flag == 0 && height<=80) {
l_flag = 1;
l_losemax = height;
}
l_line_x[height] = 0;
l_line_y[height] = height;
l_search_flag[height] = 0;
l_searchget_flage = 0;
l_lose_value++;
}
for(r_width=r_search_start; r_width<(MT9V03X_W -2); r_width++) //右边搜线
{
if( image_8[height][r_width ] !=0 && image_8[height][r_width +1]==0 && image_8[height][r_width +2]==0 && r_width<MT9V03X_W-3)
{ //白黑黑
r_line_x[height] = r_width-8; //左加偏移量
r_line_y[height] = height;
r_search_flag[height] = 1;
r_searchget_flage = 1;
break;
}
else if(r_width==MT9V03X_W -3)
{
if(r_flag==0 && height<=80)
{
r_flag=1;
r_losemax=height;
}
r_line_x[height] = MT9V03X_W -1;
r_line_y[height] = height;
r_search_flag[height] = 0;
r_searchget_flage = 0;
r_lose_value++;
break;
}
}
m_line_x[height] =(uint8_t )1.0*(l_line_x[height] +r_line_x[height])/2;
kuan[height]=r_line_x[height] - l_line_x[height];
}
}求曲率，开根号
计算曲率
选三个点计算曲率
//计算曲率 顺时针负，逆时针正
float process_curvity(uint8 x1, uint8 y1, uint8 x2, uint8 y2, uint8 x3, uint8 y3)
{
float K;
int S_of_ABC = ((x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1)) / 2;
//面积的符号表示方向
uint8 q1 = (uint8)((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
uint8 AB = my_sqrt(q1);
q1 = (uint8)((x3 - x2) * (x3 - x2) + (y3 - y2) * (y3 - y2));
uint8 BC = my_sqrt(q1);
q1 = (uint8)((x3 - x1) * (x3 - x1) + (y3 - y1) * (y3 - y1));
uint8 AC = my_sqrt(q1);
if (AB * BC * AC == 0)
{
K = 0;
}
else
K = (float)4 * S_of_ABC / (AB * BC * AC);
return K;
}
开根号
//求根
uint8 my_sqrt(uint8 x)
{
uint8 ans=0,p=0x80;
while(p!=0)
{
ans+=p;
if(ans*ans>x)
{
ans-=p;
}
p = (uint8)(p/2);
}
return(ans);
} 求x的y次方
double pow(double x, int n) {
if (n < 0)
{
n = -n;
x = 1.0 / x;
}
if (n == 0) return 1.0;
return x * pow(x, n - 1);
}复制图像
void Transfer_Camera(void)
{
for(uint8 y=0; y<MT9V03X_H; y++) //存储到一个新数组，后续处理（High为120，Width为188，刷新率为50）
{
for(uint8 x=0; x<MT9V03X_W; x++)
{
image_yuanshi[y][x] = mt9v03x_image[y][x];
}
}
}
