智能车竞赛-基于PID算法的智能车运动控制
AI-摘要
切换
小郭 GPT
AI初始化中...
介绍自己
生成本文简介
推荐相关文章
前往主页
前往tianli博客
本文共计三十多万字符，页面卡顿请耐心等待，以防止浏览器崩溃
写在前面
一年前搞智能车的时候的一些遗留资料，现在陆陆续续整理出来，当时大一的自己代码能力太弱了，又是第一次接触嵌入式，整整一个学期的投入也没能拿下比赛，现在看到这些资料不由得感慨万千，上大学以来尝试了这么多事情，时间都花费在各种各样的兴趣爱好上，折腾来折腾去，也没有找到自己真正喜欢什么，真正能在某一个领域做大做强，直到现在还是一事无成。
感性的话就不再多说了，本文主要围绕PID算法的C语言实现展开，再结合PWN波的调节来实现小车平稳运行，以及通过图像识别的参数传递实现转向功能。
电机控制部分
PID.h
/*
* PID-StableSpeed.h
*
* Created on: 2023年3月3日
* Author: 67093
*/
#ifndef CODE_PID_H_
#define CODE_PID_H_
#include "zf_common_headfile.h"
typedef struct//定义一个PID结构体
{
float kp,ki,kd;//三个系数
float error,lastError;//误差、上次误差
float integral,maxIntegral;//积分、积分限幅
float output,maxOutput;//输出、输出限幅
}PID;
//PID初始化，调节p、i、d参数的值，并且限定积分限幅以及输出限幅
void PID_Init(PID *pid,float p,float i,float d,float maxI,float maxOut);
//进行一次PID计算，参数分别为：设立的pid结构体，目标值，反馈值
void PID_Calc(PID *pid,float reference,float feedback);
#endif /* CODE_PID_H_ */
PID.c
/*
* PID-StableSpeed.h
*
* Created on: 2023年3月3日
* Author: 67093
*/
#include "zf_common_headfile.h"
#include "zf_common_typedef.h"
#include "PID.h"
void PID_Init(PID *pid,float p,float i,float d,float maxI,float maxOut)
{
pid->kp=p;
pid->ki=i;
pid->kd=d;
pid->maxIntegral=maxI;
pid->maxOutput=maxOut;
}
void PID_Calc(PID *pid,float reference,float feedback)
{
//更新数据
pid->lastError=pid->error;//将旧error存起来
pid->error=reference-feedback;//计算新error
//计算微分
float dout=(pid->error-pid->lastError)*(pid->kd);
//计算比例
float pout=pid->error*(pid->kp);
//计算积分
pid->integral+=pid->error*(pid->ki);
//积分限幅
if(pid->integral > pid->maxIntegral)
pid->integral=pid->maxIntegral;
else if(pid->integral < -pid->maxIntegral)
pid->integral=-pid->maxIntegral;
//计算输出
pid->output=pout+dout+pid->integral;
//输出限幅
if(pid->output > pid->maxOutput)
pid->output=pid->maxOutput;
else if(pid->output < -pid->maxOutput)
pid->output=-pid->maxOutput;
}stable_speed.h
/*
* stable_speed.h
*
* Created on: 2023年3月4日
* Author: 67093
*/
#ifndef CODE_STABLE_SPEED_H_
#define CODE_STABLE_SPEED_H_
#include "zf_common_headfile.h"
//传入对应电机使用的编码器编号，PID调节占空比使其速度趋于目标值（reference）
void stable_speed(int encoder, int reference);
#endif /* CODE_STABLE_SPEED_H_ */stable_speed.c
/*
* stable_speed.c
*
* Created on: 2023年3月4日
* Author: 67093
*/
#include "zf_common_headfile.h"
#include "zf_common_typedef.h"
#include "stable_speed.h"
PID pid3;//代表给编码器3使用的PID
PID pid4;//代表给编码器4使用的PID
void stable_speed(int encoder, int reference)
{
//判断编码器型号
if(encoder==3)
{
int feedback;//获取编码器上的值
feedback=-encoder_get_count(TIM4_ENCODER);
ips200_show_int(0, 280, feedback,4);//左轮子
encoder_clear_count(TIM4_ENCODER);
PID_Calc(&pid3, reference, feedback);//进行一次PID运算
speed3+=(int)pid3.output;
if(speed3<0)
speed3=300;
if(feedback==0)
pwm_set_duty(ATOM1_CH6_P02_6, 1200);
else
pwm_set_duty(ATOM1_CH6_P02_6, speed3);
}
else if(encoder==4)
{
int feedback;
feedback=-encoder_get_count(TIM6_ENCODER)/2;
ips200_show_int(40, 280, feedback,4);//右轮子
encoder_clear_count(TIM6_ENCODER);
PID_Calc(&pid4, reference, feedback);
speed4+=(int)pid4.output;
if(speed4<0)
speed4=300;
if(feedback==0)
pwm_set_duty(ATOM1_CH4_P02_4, 1200);
else
pwm_set_duty(ATOM1_CH4_P02_4, speed4);
}
}turn_speed.h
/*
* turn_speed.h
*
* Created on: 2023年3月9日
* Author: 67093
*/
#ifndef CODE_TURN_SPEED_H_
#define CODE_TURN_SPEED_H_
#include "zf_common_headfile.h"
//uint16 LCenter[MT9V03X_H]//存了60行找到的中线坐标
int16 calc_Deviation(int16* LCenter);//传入中线数组，根据前30行计算权值返回到Deviation上
void turn_speed(int16 Deviation);//传入与中线的加权差值，差值为正表示需要左转，插值为负表示需要右转
#endif /* CODE_TURN_SPEED_H_ */turn_speed.c
/*
* turn_speed.c
*
* Created on: 2023年3月9日
* Author: 67093
*/
#include "zf_common_headfile.h"
#include "turn_speed.h"
int16 calc_Deviation(int16* LCenter)
{
int16 num[40]={
1,1,1,1,1,1,1,1,1,1,
2,2,2,2,2,2,2,2,2,2,
3,3,3,3,3,3,3,3,3,3,
3,3,3,3,3,3,3,3,3,3
};
int16 Deviation=0;
for(int i=0;i<40;i++)
Deviation+=(100-LCenter[i])*num[i];
Deviation/=110;
return Deviation;
}
void turn_speed(int16 Deviation)//传入与中线的加权差值，差值为正表示需要左转，插值为负表示需要右转
{
if(Deviation>0)//右转
{
if(Deviation>0 && Deviation<=5 && speed3-speed4>600)
{
speed3=1200;
speed4=1600;
}
else if(Deviation>5 && Deviation<=10)
{
speed3+=50;
}
else if(Deviation>10 && Deviation<=20)
{
speed3+=70;
}
else
{
speed3+=90;
}
}
else if(Deviation<0)//左转
{
if(Deviation<0 && Deviation>=-5 && speed4-speed3>600)
{
speed3=1200;
speed4=1600;
}
else if(Deviation<-5 && Deviation>=-10)
{
speed4+=50;
}
else if(Deviation<=-10 && Deviation>=-20)
{
speed4+=70;
}
else
{
speed4+=90;
}
}
}moter_exercise.h
/*
* motor_exercise.h
*
* Created on: 2023年2月25日
* Author: 同位素
*/
#ifndef CODE_MOTOR_EXERCISE_H_
#define CODE_MOTOR_EXERCISE_H_
#include "zf_common_typedef.h"
#include "zf_common_headfile.h"
void Motor_init();
void Motor(int16 motor1 , int16 motor2);
#endif /* CODE_MOTOR_EXERCISE_H_ */moter_exercise.c
/*
* motor_exercise.c
*
* Created on: 2023年2月25日
* Author: 同位素
*/
#include "zf_common_typedef.h"
#include "zf_common_headfile.h"
void Motor_init()
{
/* ===========================================
pwm_init(ATOM0_CH4_P02_4,17*1000,0);
pwm_init(ATOM0_CH5_P02_5,17*1000,0);
pwm_init(ATOM0_CH6_P02_6,17*1000,0);
pwm_init(ATOM0_CH7_P02_7,17*1000,0);
system_delay_ms(1000);
=================================================
*/
gpio_init(P02_4,GPO,0, GPI_FLOATING_IN);
pwm_init(ATOM0_CH5_P02_5,17*1000,0);
gpio_init(P02_6,GPO,0, GPI_FLOATING_IN);
pwm_init(ATOM0_CH7_P02_7,17*1000,0);
system_delay_ms(1000);
}
void Motor(int16 motor1 , int16 motor2)
{
if(motor1 >= 0)
{
pwm_set_duty(ATOM0_CH4_P02_4,motor1);
pwm_set_duty(ATOM0_CH5_P02_5,0);
}
else
{
pwm_set_duty(ATOM0_CH4_P02_4,0);
pwm_set_duty(ATOM0_CH5_P02_5,-motor1);
}
if(motor1 >= 0)
{
pwm_set_duty(ATOM0_CH6_P02_6,motor1);
pwm_set_duty(ATOM0_CH7_P02_7,0);
}
else
{
pwm_set_duty(ATOM0_CH6_P02_6,0);
pwm_set_duty(ATOM0_CH7_P02_7,-motor1);
}
/* if(motor1 >= 0)
{
gpio_set_dir(P02_4,0);
pwm_set_duty(ATOM0_CH5_P02_5,motor1);
}
else
{
gpio_set_dir(P02_4,1);
pwm_set_duty(ATOM0_CH5_P02_5,-motor1);
}
if(motor1 >= 0)
{
gpio_set_dir(P02_6,0);
pwm_set_duty(ATOM0_CH7_P02_7,motor1);
}
else
{
gpio_set_dir(P02_6,1);
pwm_set_duty(ATOM0_CH7_P02_7,-motor1);
}*/
}图像识别部分
SearchLine_new.h
/*
* SearchLine_New.h
*
* Created on: 2023年2月27日
* Author: 同位素
*/
#ifndef CODE_SEARCHLINE_NEW_H_
#define CODE_SEARCHLINE_NEW_H_
#include "zf_common_headfile.h"
#include "zf_common_typedef.h"
#include "Binarization.h"
extern uint16 LCenter[MT9V03X_H];
extern uint16 L_black[MT9V03X_H] ;//左右边界
extern uint16 R_black[MT9V03X_H];
extern int break_hangshu;
extern int8 youxiajiance;
extern int8 youzhongjiance;
extern int8 huandaozhuangtai;
extern int8 shifouyoubanmaxian;
extern float xielv;
extern int16 truepianfangflag ;
extern int continueleft;
extern int16 youwushizizuoxiadian;
extern int16 shifouoyouzhongdian;
extern int16 youzhongjiance2;
extern int8 youshangjiance;
extern int16 zuochujiance;
extern int16 youwushuangduandian;
extern int16 youwuzuoshangdian;
extern int16 youwuzuoshangdian;
extern int16 youwuyoushangdian;
extern int16 youwuyouxiadian;
extern int16 youwuzuoxiadian;
void entirewhite();
void zhongxian();
void advanced_regression(int type, int startline1, int endline1, int startline2, int endline2);
void regression(int type, int startline, int endline);//最小二乘法拟合曲线，分别拟合中线，左线，右线,type表示拟合哪几条线 xy 颠倒
int My_Abs(int a, int b);//两数差的绝对值
void pianfangcal(int begin, int end, int type);//偏方判断
void monileftfuzhi(float A, float B, int start_point, int end_point);
void monirightfuzhi(float A, float B, int start_point, int end_point);
void monizhongfuzhi(float A, float B, int start_point, int end_point);
void continuepanduan();
void continuepanduanforleft();
void cumulants();//中线偏差累积
void huandaofuzhu();
void fingdoubleleft();
void fingdoubleright();
void cakkuandu();
//void podaojudge();
void mynewleftku();
void juge_left_succesive_and_if_k_limit();//判断右环岛
void R_mend_leftline_right_island(int type, int flag_R_L);//右环岛左线
void R_mend_rightline_right_island(int type, int flag_R_L);//左环岛右线
void findhuan_leftout_point(int start_point, int end_point, int RoadName) ; //找换到左下拐点 为左环岛而修改
void findhuan_rightout_point(int start_point, int end_point, int RoadName); //找环岛右下拐点
void findhuan_rightdown_point(int start_point, int end_point, int RoadName); //找环岛右下拐点
void findhuan100_rightdown_point(int start_point, int end_point, int RoadName); //找环岛右下拐点
void findhuan_leftdown_point(int start_point, int end_point, int RoadName) ; //找换到左下拐点 为左环岛而修改
void findhuan_rightmiddle_point(int start_point, int end_point) ; //找环岛右中拐点 此函数待调整
void findhuan_rightmiddle_point2(int start_point, int end_point) ;
void findhuan_leftmiddle_point(int start_point, int end_point); //找环岛左中拐点
void findhuan_rightup_point(int start_point, int end_point, int RoadName);// 找环岛右上拐点
void findhuan_leftup_point(int start_point, int end_point, int RoadName) ;//找环岛左上拐点
void findhuan_rightup_point77(int start_point, int end_point, int RoadName);// 找环岛右上拐点
void findhuan_rightup_point44(int start_point, int end_point, int RoadName);// 找环岛右上拐点
void findhuan_leftup_point77(int start_point, int end_point, int RoadName); //找环岛左上拐点
void R_mend_leftline_right_islandforleft(int type, int flag_R_L);//左环岛左线
void R_mend_rightline_right_islandforleft(int type, int flag_R_L);//左环岛右线
void find_leftup_point(int start_point, int end_point, int RoadName);//找斜十字左上拐点
void find_rightup_point(int start_point, int end_point, int RoadName);//找斜十字右上拐点
void find_rightdown_point3(int start_point, int end_point, int RoadName);//找斜十字右下
void find_leftdown_point3(int start_point, int end_point, int RoadName);//左下
void find_leftdown_point(int start_point, int end_point, int RoadName);//正十字左下拐点
void find_rightdown_point(int start_point, int end_point, int RoadName);//正十字右下拐点
void leftlinefuzhi(float A, float B, int8 start_point, int8 end_point);//补左线
void R_blackfuzhi(float A, float B, int8 start_point, int8 end_point);//补右线
void zhongxianfuzhi( int8 start_point, int8 end_point);//补中线
float FMy_Abs(float a, float b);//两点之差绝对值浮点数
void find_kuleftup_point(int start_point, int end_point);//找车库左上拐点
void find_kurightdown_point(int start_point, int end_point);//找车库右下拐点
void find_kuleftdown_point(int start_point, int end_point);//车库左下
void find_kurightup_point(int start_point, int end_point);//找车库右上拐点
void buzhongxian2_2();//补中线
void buzhongxian4();//补斜中线
void threemodeop(int modetype);//三叉路口
#endif /* CODE_SEARCHLINE_NEW_H_ */
SearchLine_new.c
一万行代码，谨慎展开
#include "zf_common_headfile.h"
//各种定义开始
//拟合
//===============检测，记得删除
int8 youxiajiance;
int8 youshangjiance;
int8 youzhongjiance;
int8 huandaozhuangtai;
int8 shifouyoubanmaxian=0;
int16 shifouoyouzhongdian=0;
float xielv=0;
int16 youwushizizuoxiadian=0;
int16 youwushuangduandian=0;
int16 youzhongjiance2=0;
int16 zuochujiance;
int16 youwuzuoshangdian=0;
int16 youwuyoushangdian=0;
int16 youwuyouxiadian=0;
int16 youwuzuoxiadian=0;
float parameterB; //斜率
float parameterA; //截距
//模拟赋值
int16 monileft[70];
int16 moniright[70];
int16 monimiddle[70];
//偏方左右
int16 pianfangleft = 0;
int16 pianfangright = 0;
int16 pianfangmid=0;
//左右双断点
int16 leftduan1 = 0;
int16 leftduan2 = 0;
int16 newopenleft = 0;
int16 truedoubleleft = 0;
int16 rightentirewhite=0;
int16 leftentirewhite=0;
int16 doublewhite=0;
int16 rightduan1 = 0;
int16 rightduan2 = 0;
int16 newopen = 0;
int16 truedoubleright = 0;
//偏方满足
int16 truepianfangflag = 0;
int16 truepianfangflagforleft=0;
//扫中线
int zhongold=93;
int leftold=0;
int rightold=0;
int rightflag[70]={0,};
int leftflag[70]={0,};
int kurightdownflag=0;
int kuleftdownflag=0;
int kuleftupflag=0;
int kurightupflag=0;
int duanhang0=0;
int duanhang1=0;
int duanhangju=0;
int break_hangshu=0;
int countprotect=0;
int turepodaoflag = 0;
int rukuflag = 0;
int protect=0;
int16 horizon=60;
int16 leftduan;
int16 rightduan;
//左右线连续
int continueleft = 0;
int continueright = 0;
//坡道
float LK0end = 0;
float LK020 = 0;
float RK0end = 0;
float RKB020 = 0;
int16 podaoflag = 0;
int16 podaoflag2=0;
int16 kuandubreak = 0;
int16 podaotype2 = 0;
int16 lasttruepodaoflag = 0;
int16 countpodao = 0;
int16 truelongflag=0;
//入库
int16 type=0;
int firstku=0;
int haveleftku = 0;
int leftkuhang = 0;
int countfirstku=0;
int lastduanhang1 = 0;
int Llastduanhang1 = 0;
int kuleft_turn_up[2]={0,};
int kuright_turn_down[2]={0,};
int kuleft_turn_down[2]={0,};
int kuright_turn_up[2]={0,};
int wankuflag=0;
int kumode=0;
int recordhang=0;
int huandaojishu=0;
int lastfirstku=0;
int16 youku=0;
int countruku = 0;
int stopruku = 0;
int count3=0;
//宽度计算
int calkuan[70];
//环岛右
int flag_find_huan_leftdown_point = 0;
int flag_find_huan_rightdown_point = 0;
int flag_find_huan_leftmiddle_point = 0;
int flag_find_huan_rightmiddle_point = 0;
int flag_find_huan_rightmiddle_point2 = 0;
int flag_find_huan_rightup_point = 0;
int flag_find_huan_leftup_point = 0;
int flag_find_huan_leftout_point=0;
int flag_find_huan_rightout_point=0;
int ROUNDISLAND = 1; //识别环岛
int huandao_memory=0;
int Rhave0=0;
int right_turn_down[2]={0,};
int right_turn_up[2]={0,};
int left_turn_down[2]={0,};
int left_turn_up[2]={0,};
int right_turn_middle[2];
int right_turn_middle2[2];
int left_turn_middle[2];
int huandao_flag_R_L=0;
int huandao_flag = 0;
int huandao_procedure_variable = 0;
int huandao_memoryforleft=0;
int flag_rukou2 = 0;
int flag_rukou = 0;
int last_memory=0;
//右环岛左线
int jiansu_flag = 0;
int flag_blank_out_huandao = 0;
//右环岛右线
//左环岛主体
int lcenter_5=0;
int last_memoryforleft=0;
int flag_find_huan_leftout_point;
int left_turn_out [2];
int leftdown15white = 0;
//斜入十字
int rightdownflag3=0;
int leftdownflag3=0;
int rightupflag3=0;
int leftupflag3=0;
int firstmid=0;
int8 CROSSROAD = 0;
int bigup=0;
//正入十字
int rightdownflag=0;
int rightupflag=0;
int leftdownflag=0;
int leftupflag=0;
int secondmid=0;
int buzhongxianmax = 0;
int buzhongxianmin = 0;
//小s
int turesmallsflag=0;
int countstoplongg = 0;
int flag_centre_right_point = 0;
int flag_centre_left_point = 0;
int centre_left_point[2];
int centre_right_point[2];
int swrong = 0;
int overflag = 0;
int flag_middle_S=0;
int flag_small_S=0;
int findturn = 0;
int finddirection = 0;//1左转弯 2右转弯
int min;
int max;
int LongLCenter[70]={0,};
int breakwrong = 0;
//中s
int trueflag_middle_S = 0;
int last1trueflag_middle_S=0;
int last2trueflag_middle_S=0;
int last3trueflag_middle_S=0;
//长直道
int countstoplong = 0;
int stoplong= 0;
int overlong=0;
int overlongwrong=0;
int changzhenshu=24;
int lastturesmallsflag=0;
int stopsmalls=0;
int countstopsmalls=0;
int truelong1=0;
int truelong2=0;
int turedian=0;
int kthird=0;
int lasttruelong2 = 0;
int longflag=0;
int thirdmid=0;
int trueshortflag=0;
//环岛快速进入
float R020K = 0;
float L020K=0;
//十字防断
int bannothing=0;
int LCenterrecord[70];
int leftrecord[70];
int rightrecord[70];
int countbannothing = 0;
int16 liangbiandiuxian=0;
//二次十字
int se_thrmid = 0;
//三岔路口
int16 threemode=0;
int16 countleft10;
int16 countright10 ;
int16 threekuanduEQ ;
int16 secondleft;
int16 firstleft;
int16 threeleftdownflag;
int16 threerightdownflag;
int16 threeleftdown[2]={0,};
int16 endmidddle=0;
int16 givetime=0;
int16 end = 0;
int16 threerightdown[2]={0,};
int16 threeroadextrari;
int16 threeroadextra;
int16 threeroadextrale;
//各种计数
int huandaotype=3;
//累差
int16 cumulants1;
int16 cumulants2;
int16 openright;
int16 endright;
uint16 LCenter[MT9V03X_H];
uint16 R_black[MT9V03X_H];
uint16 L_black[MT9V03X_H];
//=======================================中线====================================
void zhongxian()
{
int hang;
int lie;
int guixian = 0;
int zhongold2 = 0;
//第一次扫线，获取0-60行左右线的值
for (hang = 0; hang < horizon; hang++)
{
for (lie = zhongold; lie >= 3; lie--)
{
if (lie >= (int)185) lie = 184;
if (Pixels[hang][ lie - 2] == 0&&Pixels[hang][ lie - 3] == 0&&Pixels[hang][ lie - 1] == 0 && Pixels[hang][ lie] == 0 && Pixels[hang][lie + 1] == 1) //黑白
{
R_black[hang] = (unsigned char)(lie + 1);
rightold = (int)(lie + 1);
rightflag[hang] = 1;
break;
}
else
{
R_black[hang] = 0;
rightflag[hang] = 0;
}
}
for (lie = zhongold; lie < 185; lie++)
{
if (lie == 0) lie = 1;
if (Pixels[hang][ lie + 1] == 0 && Pixels[hang][ lie] == 0 && Pixels[hang][ lie - 1] == 1)
{
L_black[hang] = (unsigned char)(lie - 1);
leftold = (int)(lie - 1);
leftflag[hang] = 1;
break;
}
else
{
L_black[hang] = 186;
leftflag[hang] = 0;
}
}
//下一行的扫线在上一行的基础上向左右扫
zhongold = (int)((L_black[hang] + R_black[hang]) / 2);
}
for (hang = 0; hang < horizon; hang++)
{ LCenter[hang] = (unsigned char)((L_black[hang] + R_black[hang]) / 2); }
/*二次扫线*/
duanhang0 = 0;
duanhang1 = 0;
leftduan=0;
rightduan=0;
for (hang = 1; hang < horizon; hang++) //扫断点0
{
/*
断行0的原理:由于摄像头视角，呈现出的赛道应该是近大远小的，如果远
处的行 宽度比近处的大，那么认为出现第一个断行。(大家可以想一下十
字路口的下方 赛道宽度变宽)
*/if(My_Abs(L_black[hang],L_black[hang+1])>4&&My_Abs(L_black[hang],L_black[hang+2])>4)
{
leftduan=1;
}
if(My_Abs(R_black[hang],R_black[hang+1])>4&&My_Abs(R_black[hang],R_black[hang+2])>4)
{
rightduan=1;
}
if ((L_black[hang] - R_black[hang]) - (L_black[hang - 1] - R_black[hang - 1]) >= 4&&hang>=13)
{
duanhang0 = hang - 1;
duanhangju = (L_black[hang - 1] - R_black[hang - 1]);
break;
}
}
if (duanhang0 > 10)
{
/*
如果扫描到断行0，那么从断行0开始的图像可能有问题，
所以这里选择固定终止往下扫线，寻找断行1的存在。
(断行1的原理，当宽度再次变小且小于断行0处的宽度，
认为是正确的断行1，大家可以想一下十字路口的上半段)
*/
zhongold2 = LCenter[duanhang0 - 7];
for (hang = (int)(duanhang0 + 3); hang < horizon; hang++) //固定中值扫线
{
for (lie = (int)zhongold2; lie >= 3; lie--) //扫右线
{
if (lie >= 185) lie = 184;
if (Pixels[hang][ lie - 2] == 0&&Pixels[hang][ lie - 3] == 0&&Pixels[hang][ lie - 1] == 0 && Pixels[hang][ lie] == 0 && Pixels[hang][lie + 1] == 1) //白黑黑
{
R_black[hang] = (unsigned char)(lie + 1);
rightold = (int)(lie + 1);
rightflag[hang] = 1;
break;
}
else
{
R_black[hang] = 0;
rightflag[hang] = 0;
}
}
for (lie = (int)zhongold2; lie < 185; lie++) //扫左线
{
if (lie == 0) lie = 1;
if (Pixels[hang][ lie + 1] == 0 && Pixels[hang][ lie] == 0 && Pixels[hang][ lie - 1] == 1)
{
L_black[hang] = (unsigned char)(lie - 1);
leftold = (int)(lie - 1);
leftflag[hang] = 1;
break;
}
else
{
L_black[hang] = 186;
leftflag[hang] = 0;
}
}
}
}
for (hang = (int)(duanhang0 + 3); hang < horizon; hang++)//扫断点1
{
/*
计算是否存在断行1
*/
if ((L_black[hang] - R_black[hang]) < duanhangju-15&&L_black[hang]<=180&&R_black[hang]>=5)
{
duanhang1 = (signed short int)hang;
// SetText(" 找到断行1 " + duanhang1);
// SetText(" duanhangju1 " + calkuan[duanhang1]);
break;
}
}
if (duanhang0 > 10 && duanhang1 != 0)
{
/*
找到断行1，开始进入二次扫线模式
二次扫线思路:从断行1处开始使用断行0的中值进行扫线，动态继承中值往下扫。
*/
zhongold2 = LCenter[duanhang0 - 7];
int gudingtime = 1;
for (hang = (int)(duanhang1); hang < horizon; hang++) //二次扫线
{
for (lie = (int)zhongold2; lie >= 3; lie--) //扫右线
{
if (lie >= 185) lie = 184;
if (Pixels[hang][ lie - 1] == 0 && Pixels[hang][ lie] == 0 && Pixels[hang][lie + 1] == 1) //白黑黑
{
R_black[hang] = (unsigned char)(lie + 1);
rightold = (int)(lie + 1);
rightflag[hang] = 1;
break;
}
else
{
R_black[hang] = 0;
rightflag[hang] = 0;
}
}
for (lie = (int)zhongold2; lie < 185; lie++) //扫左线
{
if (lie == 0) lie = 1;
if (Pixels[hang][ lie + 1] == 0 && Pixels[hang][ lie] == 0 && Pixels[hang][lie - 1] == 1)
{
L_black[hang] = (unsigned char)(lie - 1);
leftold = (int)(lie - 1);
leftflag[hang] = 1;
break;
}
else
{
L_black[hang] = 186;
leftflag[hang] = 0;
}
}
if (gudingtime != 0) gudingtime = gudingtime + 1;
if (gudingtime == 8) gudingtime = 0;
if (gudingtime != 0) zhongold2 = LCenter[duanhang0 - 7];
if (gudingtime == 0) zhongold2 = (L_black[hang] + R_black[hang]) / 2;
}
}
// 终止
//存入显示数组中
for (hang = 0; hang < 69; hang++) //去掉杂点
{
LCenter[hang] = (unsigned char)((L_black[hang] + R_black[hang]) / 2);
if (hang > 5)
{
if (Pixels[hang][ LCenter[hang]] == 0 && Pixels[hang + 1][LCenter[hang]] == 0&&huandao_memory!=4&&huandao_memoryforleft!=4)
{
for (guixian = hang; guixian < 70; guixian++)
{
LCenter[hang] = LCenter[hang - 1];
L_black[guixian] = 0;
R_black[guixian] = 0;
}
break;
}
}
}
int j;
break_hangshu = 0;
for (j = 0; j < horizon; j++)
{
if ((Pixels[j][ LCenter[j]]) == 0 && (Pixels[j + 1][ LCenter[j]]) == 0&& huandao_memory != 4 && huandao_memoryforleft != 4)
{
break_hangshu = (int16)j;
//last_break_hangshu = break_hangshu;
//也就是说二十行一下是不会break的
if (break_hangshu >= 20) //防止在一开始就break
{
break;
}
}
if ((Pixels[j][ LCenter[j]]) == 0 && (Pixels[j + 1][ LCenter[j+1]]) == 0 && (Pixels[j + 2][ LCenter[j+2]]) == 0 && (Pixels[j + 3][ LCenter[j+3]]) == 0 && (huandao_memory == 4 || huandao_memoryforleft == 4))
{
break_hangshu = (int16)j;
//last_break_hangshu = break_hangshu;
//也就是说二十行一下是不会break的
if (break_hangshu >= 20) //防止在一开始就break
{
break;
}
}
}
if (break_hangshu == 0) break_hangshu = horizon;
zhongold = LCenter[4];
if (break_hangshu >= 4)
{
int calendleft =0;
int calendright = 0;
int i;
for (i = break_hangshu - 1; i >= 4; i--)
{
if (My_Abs(L_black[i], L_black[i - 1]) <= 4&&L_black[i]!=186)
{
//计算左边线的最终有效行数
calendleft = i-1;
// SetText("calendleft " + calendleft);
break;
}
}
for (i = calendleft; i >= 4; i--)
{
if (L_black[i] == L_black[i - 1]) calendleft = i - 1;
if (L_black[i] != L_black[i - 1]) break;
}
for (i = break_hangshu - 1; i >= 4; i--)
{
if (My_Abs(R_black[i], R_black[i - 1]) <= 4&&R_black[i]!=0)
{
//计算右边线的最终有效行数
calendright = i-1;
// SetText("calendright " + calendright);
break;
}
}
for (i = calendright; i >= 4; i--)
{
if (R_black[i] == R_black[i - 1]) calendright = i - 1;
if (R_black[i] != R_black[i - 1]) break;
}
// SetText("break_hangshu " + break_hangshu);
// SetText("calendleft " + calendleft);
// SetText("calendright " + calendright);
if (calendleft >= 45) calendleft = 45;
if (calendright >= 45) calendright = 45;
//求开始点
//int calbeginleft=0;
//int calbeginright=0;
//for (i = 0; i <= 12; i++)
//{
// if (R_black[i] != 0)
// {
// calbeginright = i;
// break;
// }
//}
//for (i = 0; i <= 12; i++)
//{
// if (L_black[i] != 186)
// {
// calbeginleft = i;
// break;
// }
//}
//SetText("calbeginleft " + calbeginleft);
// SetText("calbeginright " + calbeginright);
// SetText(" L_black[calbeginleft] " + L_black[calbeginleft]);
// SetText("L_black[calendleft] " + L_black[calendleft]);
//SetText(" R_black[calbeginright] " + R_black[calbeginright]);
//SetText(" R_black[calendright] " + R_black[calendright]);
/*
下面的代码是用来计算环岛的相关参数，进行辅助判断。
*/
if (calendleft >= 20)
{
pianfangcal(0, calendleft, 1);
}
if (calendleft < 20)
{
pianfangcal(0, break_hangshu - 3, 1);
}
if (calendright >= 20)
{
pianfangcal(0, calendright, 2);
}
if (calendright < 20)
{
pianfangcal(0, break_hangshu - 3, 2);
}
}
countprotect = 0;
if (turepodaoflag == 0 && rukuflag == 0) //坡道不退出 入库刹车10帧后自行退出
{
for (j = 0; j <= 185; j++) //退出
{
if ((Pixels[0][ j]) == 0)
{
countprotect = countprotect + 1;
}
if (countprotect == 186) { protect = 1; break; }
}
}
// rightopen = R_black[0];
// leftend = L_black[0];
}
//=======================================中线====================================
//========================================最小二乘法================================
void advanced_regression(int type, int startline1, int endline1, int startline2, int endline2)
{
int i = 0;
int sumlines1 = endline1 - startline1;
int sumlines2 = endline2 - startline2;
int sumX = 0;
int sumY = 0;
float averageX = 0;
float averageY = 0;
float sumUp = 0;
float sumDown = 0;
if (type == 0) //拟合中线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += LCenter[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += LCenter[i];
}
averageX =(float)( sumX / (sumlines1 + sumlines2)); //x的平均值
averageY = (float)(sumY / (sumlines1 + sumlines2)); //y的平均值
for (i = startline1; i < endline1; i++)
{
sumUp += (LCenter[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (LCenter[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 1) //拟合左线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += L_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += L_black[i];
}
averageX =(float)( sumX / (sumlines1 + sumlines2)); //x的平均值
averageY = (float)(sumY / (sumlines1 + sumlines2)); //y的平均值
for (i = startline1; i < endline1; i++)
{
sumUp += (L_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (L_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 2) //拟合右线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += R_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += R_black[i];
}
averageX =(float)( sumX / (sumlines1 + sumlines2)); //x的平均值
averageY = (float)(sumY / (sumlines1 + sumlines2)); //y的平均值
for (i = startline1; i < endline1; i++)
{
sumUp += (R_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (R_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
xielv=sumDown;
}
}
void regression(int type, int startline, int endline)//最小二乘法拟合曲线，分别拟合中线，左线，右线,type表示拟合哪几条线 xy 颠倒
{
int i = 0;
int sumlines = endline - startline;
int sumX = 0;
int 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 += LCenter[i];
}
if (sumlines != 0)
{
averageX = (float)(sumX / sumlines); //x的平均值
averageY =(float)( sumY / sumlines); //y的平均值
}
else
{
averageX = 0; //x的平均值
averageY = 0; //y的平均值
}
for (i = startline; i < endline; i++)
{
sumUp += (LCenter[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 1)//拟合左线
{
for (i = startline; i < endline; i++)
{
sumX += i;
sumY += L_black[i];
}
if (sumlines == 0) sumlines = 1;
averageX = (float)(sumX / sumlines); //x的平均值
averageY =(float)( sumY / sumlines); //y的平均值
for (i = startline; i < endline; i++)
{
sumUp += (L_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 2)//拟合右线
{
for (i = startline; i < endline; i++)
{
sumX += i;
sumY += R_black[i];
}
if (sumlines == 0) sumlines = 1;
averageX = (float)(sumX / sumlines); //x的平均值
averageY =(float)( sumY / sumlines); //y的平均值
for (i = startline; i < endline; i++)
{
sumUp += (R_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
}
//========================================最小二乘法================================
//====================================求两数之差绝对值开始===================================
int My_Abs(int a, int b)
{
if ((a - b) > 0)
return ((int)(a - b));
else return ((int)(b - a));
}
//====================================求两数之差绝对值开始===================================
//====================================偏方================================================
void pianfangcal(int begin, int end, int type)
{
int i = 0;
if (type == 1)//左线拟合差平方计算
{
pianfangleft = 0;
regression (1, begin, end);
monileftfuzhi(parameterA, parameterB,(int) begin,(int) end);
for (i = begin; i <= end; i++)
{
pianfangleft = pianfangleft + (L_black[i] - monileft[i]) * (L_black[i] - monileft[i]);
}
pianfangleft = pianfangleft / (end - begin + 1);
}
if (type == 2)//右线拟合差平方计算
{
pianfangright = 0;
regression(2, begin, end);
monirightfuzhi(parameterA, parameterB,(int) begin,(int) end);
for (i = begin; i <= end; i++)
{
pianfangright = pianfangright + (R_black[i] - moniright[i]) * (R_black[i] - moniright[i]);
}
pianfangright = pianfangright / (end - begin + 1);
}
if (type == 0)//右线拟合差平方计算
{
pianfangmid = 0;
regression(0, begin, end);
monizhongfuzhi(parameterA, parameterB, (int)begin, (int)end);
int fangjun = 0;
int junfang = 0;
for (i = begin; i <= end; i++)
{
fangjun = fangjun + (LCenter[i]) * (LCenter[i]);
}
fangjun= fangjun / (end - begin + 1);
for (i = begin; i <= end; i++)
{
junfang = junfang + (LCenter[i]);
}
junfang = junfang / (end - begin + 1);
junfang = junfang * junfang;
pianfangmid = fangjun - junfang;
}
}
//====================================偏方================================================
//====================================模拟===============================================//获得斜率与截距之后模拟出所要的线
void monileftfuzhi(float A, float B, int start_point, int end_point)
{
int m;
for (m = start_point; m <= end_point; m++)
{
if((B * m + A)>=255) monileft[m]=255;
if((B * m + A)<=0) monileft[m]=0;
else if(0<(B * m + A)&&(B * m + A)<255)monileft[m] = (int)(B * m + A);
}
}
void monirightfuzhi(float A, float B, int start_point, int end_point)
{
int m;
for (m = start_point; m <= end_point; m++)
{
if((B * m + A)>=255) moniright[m]=255;
if((B * m + A)<=0) moniright[m]=0;
else if(0<(B * m + A)&&(B * m + A)<255)moniright[m] = (int)(B * m + A);
}
}
void monizhongfuzhi(float A, float B, int start_point, int end_point)
{
int m;
for (m = start_point; m <= end_point; m++)
{
if ((B * m + A) >= 255) monimiddle[m] = 255;
if ((B * m + A) <= 0) monimiddle[m] = 0;
else if (0 < (B * m + A) && (B * m + A) < 255) monimiddle[m] = (int)(B * m + A);
}
}
//====================================模拟==================================================
//=================================左右线连续开始==========================================
void continuepanduan()
{
int i = 0;
continueleft = 1;
if (break_hangshu >= 40)
{
//如果所有行中出现大于5的跳变，认为不连续
//如果第15行边线仍不在1-185范围内，认为不连续
for (i = 5; i <= 40; i++)
{
if (My_Abs(L_black[i], L_black[i + 1]) > 5&&My_Abs(L_black[i], L_black[i + 2]) > 5)
{
continueleft = 0;
// SetText("左连续跳出点 " + i);
break;
}
else
{
continueleft = 1;
break;
}
}
}
if (break_hangshu < 40)
{
for (i = 5; i <= break_hangshu - 6; i++)
{
if (My_Abs(L_black[i], L_black[i + 1]) > 5&&My_Abs(L_black[i], L_black[i + 2]) > 5)
{
continueleft = 0;
// SetText("左连续跳出点 " + i);
break;
}
else
{
continueleft = 1;
break;
}
}
}
// SetText("continueleft" + continueleft);
}
void continuepanduanforleft()
{
int i = 0;
continueright = 1;
if (break_hangshu >= 40)
{
//此处原理同上
for (i = 5; i <= 38; i++)
{
if (My_Abs(R_black[i], R_black[i + 1]) > 5&&My_Abs(R_black[i], R_black[i + 2]) > 5)
{
continueright = 0;
break;
}
else
{
continueright = 1;
break;
}
}
}
if (break_hangshu < 40)
{
for (i = 5; i <= break_hangshu - 6; i++)
{
if (My_Abs(R_black[i], R_black[i + 1]) > 5&&My_Abs(R_black[i], R_black[i + 2]) > 5)
{
continueright = 0;
break;
}
else
{
continueright = 1;
break;
}
}
}
}
//=================================左右线连续结束==========================================
//=====================================累差开始=============================
/*
顾名思义 累差就是值许多行中线所累积的差值
(行与行之间取绝对值，防止正负抵消)
这个条件在小s判断的时候应用有着比较明显的效果
*/
void cumulants()
{
int i;
cumulants1 = 0;
cumulants2 = 0;
for (i = 6; i <= 25; i++)
{
cumulants2 = cumulants2 + My_Abs(LCenter[i], LCenter[i - 1]);
}
for (i = 1; i <= 40; i++)
{
cumulants1 = cumulants1 + My_Abs(LCenter[i], LCenter[i - 1]);
}
/*
下面的代码计算右边线的有效起始行和结束行
*/
openright = 0;
endright = 0;
if (R_black[0] == 0)
{
for (i = 0; i < 50; i++)
{
if (R_black[i] != 0)
{
openright = (int16)i;
break;
}
}
}
int hang;
for (hang = openright; hang < 65; hang++) //去掉杂点
{
if (R_black[hang] == 0 && R_black[hang + 1] == 0)
{
endright = hang - 1;
break;
}
}
}
//=====================================累差结束=============================
//====================================环岛辅助判断=============================
/*
环岛辅助条件用于决定是否进入环岛判断函数
下面的辅助条件主要有两个方面：
(1)环岛所在边在赛道上必须有两个断点 比如0-20连续 20-23空缺
24-30连续 31-35空缺 36-40连续 这两个空缺处就是环岛的入口和出口
(2)对于环岛来说 存在入口和出口的边的偏方差(我自己定义的函数，可以
直接类比方差)很大 而另一边则几乎是一条直线，偏房差很小
下面的代码主要就是在于寻找环岛的双端点和计算偏方差的比值
*/
void huandaofuzhu()
{
truepianfangflag = 0;
truepianfangflagforleft = 0;
if (turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && rukuflag == 0)
{
/*
环岛双断点在此处寻找但并没有直接应用，具体使用请跳转至环岛函数查看
*/
fingdoubleright(); //第一防止误判双断点
fingdoubleleft();
if (pianfangleft == 0 && pianfangright >= 39)
{
truepianfangflag = 1; //第二防止误判偏离平方
}
if (pianfangleft <= 10 && pianfangright >= 30 && pianfangright <= 60)
{
truepianfangflag = 1; //第二防止误判偏离平方
}
if (pianfangleft <= 30 && pianfangright >= 80 && pianfangright <= 180)
{
truepianfangflag = 1; //第二防止误判偏离平方
}
if (pianfangleft <= 4 && pianfangright >= 45)
{
truepianfangflag = 1; //第二防止误判偏离平方
}
if (pianfangleft <= 6 && pianfangright >= 60)
{
truepianfangflag = 1; //第二防止误判偏离平方
}
if (pianfangleft <= 40 && pianfangright >= 180 && pianfangright<= 280)
{
truepianfangflag = 1; //第二防止误判偏离平方
}
if (pianfangleft <= 20 && pianfangright >= 180 )
{
truepianfangflag = 1; //第二防止误判偏离平方
}
if (pianfangleft <= 60 && pianfangleft>=40&& pianfangright <= 110 && pianfangright >= 90)
{
truepianfangflag = 1; //第二防止误判偏离平方
}
/*
下面这些判断条件是我不断地跑赛道得出来的，大家使用的时候
需要根据自己摄像头的实际情况去调整。
*/
if (pianfangleft >= 60 && pianfangright <= 15 ) truepianfangflagforleft = 1; //第二防止误判偏离平方
if (pianfangleft >= 105 && pianfangright <= 18 && pianfangleft<=400) truepianfangflagforleft = 1; //第二防止误判偏离平方
if (pianfangleft >= 45 && pianfangright <= 4 && pianfangleft <= 400) truepianfangflagforleft = 1; //第二防止误判偏离平方
if (pianfangleft >= 300 && pianfangright <= 100 && pianfangleft <= 400) truepianfangflagforleft = 1; //第二防止误判偏离平方
}
}
//====================================环岛辅助判断=============================
//================================双断点开始======================================
void fingdoubleleft()
{
int i = 0;
leftduan1 = 0;
leftduan2 = 0;
newopenleft = 0;
truedoubleleft = 0;
if (break_hangshu >= 10)
{
for (i = 4; i <= break_hangshu ; i++)
{
if (My_Abs(L_black[i], L_black[i + 1]) > 5)
{
leftduan1 = i + 1;
break;
}
if (i>=12&&L_black[i]==186&& L_black[i+1] != 186)
{
leftduan1 = i + 1;
break;
}
if (i == 28) break;
}
if (leftduan1 != 0)
{
for (i = leftduan1 + 2; i <= break_hangshu ; i++)
{
if (My_Abs(L_black[i], L_black[i + 1]) < 4&& My_Abs(L_black[i+1], L_black[i + 2])<4)
{
newopenleft = (int16)i;
break;
}
}
}
if (newopenleft != 0)
{
for (i = newopenleft; i <= break_hangshu ; i++)
{
if (My_Abs(L_black[i], L_black[i + 1]) > 5 && (i + 1) - leftduan1 >= 5 )
{
leftduan2 = i + 1;
break;
}
}
}
if (leftduan2 != 0 && leftduan2 >= leftduan1 + 5 )
{
truedoubleleft = 1;
}
}
}
void fingdoubleright()
{
int i = 0;
rightduan1 = 0;
rightduan2 = 0;
newopen = 0;
truedoubleright = 0;
youwushuangduandian=truedoubleright;
if (break_hangshu >= 10)
{
for (i = 4; i <= break_hangshu ; i++)
{
if (My_Abs(R_black[i], R_black[i + 1]) > 10&&My_Abs(R_black[i+2], R_black[i]) > 10&&My_Abs(R_black[i], R_black[i + 3]) > 10&&My_Abs(R_black[i], R_black[i + 4]) > 10)
{
rightduan1 = i + 1;
break;
}
if (i == 28) break;
}
if (rightduan1 != 0)
{
for (i = rightduan1 + 1; i <= break_hangshu ; i++)
{
if (My_Abs(R_black[i], R_black[i + 1]) < 4&& My_Abs(R_black[i+1], R_black[i + 2]) < 4)
{
newopen = (int16)i;
break;
}
}
}
if (newopen != 0)
{
for (i = newopen; i <= break_hangshu ; i++)
{
if (My_Abs(R_black[i], R_black[i + 1]) > 10&&My_Abs(R_black[i+2], R_black[i ]) > 10&&My_Abs(R_black[i], R_black[i + 3]) > 10&&My_Abs(R_black[i], R_black[i + 4]) > 10&&(i+1)- rightduan1>=5)
{
rightduan2 = i + 1;
break;
}
}
}
if (rightduan2 != 0&& rightduan2>= rightduan1+5)
{
truedoubleright = 1;
}
}
}
//=============================双断点结束====================================
//==============================计算宽度开始===================================
/*
这个大家应该很好理解，赛道的宽度在不少情况下都有着比较大的用处
但我这里也需要指出的弊端是：过于依赖赛道宽度的代码普适性不高
调整起来会比较麻烦。
*/
void cakkuandu()
{
int i;
for (i = 0; i <= 69; i++)
{
calkuan[i] = 0;
}
for (i = 0; i <= break_hangshu; i++)
{
calkuan[i] = L_black[i] - R_black[i];
}
for (i = 0; i <= break_hangshu; i++)
{
//SetText("行数+宽度" + " " + i + " " + calkuan[i]);
}
}
//==============================计算宽度结束===================================
////===================================坡道开始================================================不知道有没有坡道，后期再调
//
//
///////////////////////////////////////有问题
//void podaojudge()
//{
//
// if (countpodao != 0) countpodao = countpodao + 1;
// if (countpodao == 30) countpodao = 0;
//
// if (countpodao == 0|| countpodao>20)
// {
// int i = 0; //50大于70 30大于100 20大于110
// float kerrorL = 0;
// float kerrorR = 0;
// int end = 0;
// int hang;
// /*
// 下面这一部分用于获得赛道的起始和终止行数，在坡道判定中会有用处。
// */
// for (hang = 1; hang < 69; hang++) //去掉杂点
// {
//
// if (LCenter[hang] == 0 && LCenter[hang + 1] == 0)
// {
//
// end = hang - 1;
// break;
// }
// if (My_Abs(LCenter[hang], LCenter[hang + 1]) > 20 && hang > 5)
// {
//
// end = hang - 1;
// break;
// }
// }
//
// //int begin = 0;
// //for (i = 0; i < 50; i++)
// //{
// //if(R_black[i]==0&& R_black[i+1] != 0)
// // {
// // begin = i + 1;
// // break;
// // }
// //}
// //SetText("begin " + begin);
//
//
//
// int beginleft =1 ;
// if (L_black[0] == 186)
// {
// for (i = 0; i < 50; i++)
// {
// if (L_black[i] == 186 && L_black[i + 1] == 186 && L_black[i + 2] != 186)
// {
// beginleft = i + 2;
// break;
// }
// }
// }
// int beginright = 1;
// if (R_black[0] == 0)
// {
// for (i = 0; i < 50; i++)
// {
// if (R_black[i] == 0 && R_black[i + 1] == 0 && R_black[i + 2] != 0)
// {
// beginright = i + 2;
// break;
// }
// }
// }
// if (beginleft >= 17) beginleft = 17;
// if (beginright > 17) beginright = 17;
///*
//敲黑板!
//下面这一段基本上可以说是整个坡道判定的核心
//比较0-20 0-end 行的斜率差值，来做出坡道的判定
//这里的理论依据是，坡道情况下，整个赛道会被抬高，你会明显看到坡道那一段
//的斜率和前面普通赛道上斜率出现不同。就像两个线段接在一起一样
//
//
//
//*/
//
// regression(1, beginleft, break_hangshu - 5);
//
// LK0end = parameterB;
//
// advanced_regression(1, beginleft, beginleft+(20- beginleft)/3, beginleft + (20 - beginleft) / 3*2, 20);
//
// LK020 = parameterB;
// regression(2, beginright, break_hangshu - 5);
//
// RK0end = parameterB;
//
// advanced_regression(2, beginright, beginright+(20- beginright)/3, beginright + (20 - beginright) / 3*2, 20);
//
// RKB020 = parameterB;
// kerrorL = FMy_Abs(LK0end, LK020);
// kerrorR = FMy_Abs(RK0end, RKB020);
///*因为下面要利用赛道宽度值 这里寻找左右边线都存在的最后一行 留下标记
// (只存在一边边线的行数 宽度不太有意义)*/
// int kuandubreakhang=0;
// for (i = 69; i > 0; i--)
// {
// if (L_black[i] != 0 && R_black[i] != 0)
// {
// kuandubreakhang = i;
//
// break;
// }
// }
// if (kuandubreakhang >= 32)
// { kuandubreak = L_black[kuandubreakhang ] - R_black[kuandubreakhang ]; }
// if (kuandubreakhang < 32&& kuandubreakhang >= 22)
// { kuandubreak = L_black[15] - R_black[15]; }
// if (kuandubreakhang < 22)
// {
// kuandubreak = 0;
// }
//
// int midddlecontinue = 1;
// /*
// 显然，坡道的情况下，中线肯定是连续的，midddlecontinue == 1 很重要
// */
// for (i = 1; i < break_hangshu - 10; i++)
// {
// if (LCenter[i] - LCenter[i - 1] > 4) { midddlecontinue = 0; }
// }
///*
//下面语句判断的思路大同小异
//(1)不在环岛状态机中
//(2)找到0-20 0-end 左右线斜率的关系
//(3)宽度值必须要满足一定的条件 (因为坡道也是赛道 宽度要足够宽)
//(4)持续好几帧图像
//
//*/
//
// /*第一种 两米的坡道*/
// if (huandao_memory == 0 && huandao_memoryforleft == 0&&
// turepodaoflag == 0 && truepodaotype1 == 0 && truepodaotype2 == 0 && RKB020 >= (RK0end + 0.2) && RKB020 <= (RK0end + 0.8) && RKB020 > 1 && RKB020 < 3 &&
// LK020 <= LK0end - 0.1 && LK020 <= -0.6&& LK020 >= -1.2 && LK020 >= LK0end - 0.7 && continueleft == 1 &&
// continueright == 1 && pianfangleft <= 50 && pianfangright <= 50 && calkuan[break_hangshu - 3] > 40 && calkuan[break_hangshu - 4] > 40 && calkuan[break_hangshu - 5] > 40)
// {
// podaoflagonemeter = podaoflagonemeter + 1;
// if (podaoflagonemeter == 3)
// {
// turepodaoflag = 1;
// truelongflag = 0;
// podaoflagonemeter = 0;
// truepodaotype1 = 1;
// truepodaotype2 = 0;
// podaotype = 1;
//
//
// }
//
// }
// /*第一种 两米的坡道*/
// if (huandao_memory == 0 && huandao_memoryforleft == 0&&
// turepodaoflag == 0 && truepodaotype1 == 0 && truepodaotype2 == 0 && RKB020 >= (RK0end + 0.05) && RKB020 <= (RK0end + 0.9) && RKB020 >1&& RKB020 < 3&&
// LK020 <= LK0end - 0.2 && LK020 <= -0.2 && LK020 >= -2.4 && LK020>= LK0end - 0.9 &&continueleft == 1 &&
// continueright == 1 && pianfangleft<=50 && pianfangright <= 50&&calkuan[break_hangshu-3]>40 && calkuan[break_hangshu - 4] > 40 && calkuan[break_hangshu - 5] > 40)
// {
// podaoflag = podaoflag + 1;
// if (podaoflag == 3)
// {
// turepodaoflag = 1;
// truelongflag = 0;
// podaoflag = 0;
// truepodaotype1 = 1;
// truepodaotype2 = 0;
// podaotype = 2;
//
// }
//
// }
///*
//弯道接坡道 不可能有时间足够的帧数去判别
//思路就是 因为40行是图像的尽头了 一般宽度20 30
//如果40行左右宽度仍然大于50 60 就认为弯接坡道
//进入坡道状态机
//
//*/
//
// //第三种 弯接两米的坡道
// if ( turepodaoflag == 0 && truepodaotype1 == 0 && truepodaotype2 == 0
// &&(L_black[39] - R_black[39] >= 63) && (L_black[40] - R_black[40] >= 63 ) &&
// (L_black[41] - R_black[41] >= 63 )
// && (L_black[43] - R_black[43] >= 50||L_black[44] - R_black[44] >= 50 ))
// {
// int first10=1;
// int second20=1;
// int third30=1;
// int fourth40=1;
// for (i = 0; i < 10; i++)
// {
// if (L_black[i]-R_black[i] < 140)
// {
// first10 = 0;
// break;
// }
// }
//
// if (first10 == 1)
// {
// for (i = 10; i < 20; i++)
// {
// if (L_black[i] - R_black[i] < 130)
// {
// second20 = 0;
// break;
// }
// }
// }
//
// if (second20 == 1)
// {
// for (i = 20; i < 30; i++)
// {
// if (L_black[i] - R_black[i] < 100)
// {
// third30 = 0;
// break;
// }
// }
// }
//
// if (third30 == 1)
// {
// for (i = 30; i < 40; i++)
// {
// if (L_black[i] - R_black[i] < 70)
// {
// fourth40 = 0;
// break;
// }
// }
// }
//
// if (first10 == 1 && second20 == 1 && third30 == 1 && fourth40 == 1&&huandao_memory == 0 && huandao_memoryforleft == 0)
// {
// podaoflagquick = podaoflagquick + 1;
//
// if (podaoflagquick == 2)
// {
// turepodaoflag = 1;
// truelongflag = 0;
// podaoflagquick = 0;
// truepodaotype1 = 1;
// truepodaotype2 = 0;
// podaotype = 2;
//
// countpodao1flag = 0;
// }
// }
//
// }
//
//
//
//
///*判断到坡道之后
//进入坡道状态机的第二部分
//下面的部分包括
//上坡一定程度锁定中值
//坡顶开启扫线
//下坡状态
//
//*/
//
//
///*
//首先，判断到坡道之后十帧图像之内 认为是基本的上坡过程 状态持续保持为1
//
//*/
// if (turepodaoflag == 1 && countpodao1flag<=10&& truepodaotype1==1&&truepodaotype2 == 0)
// {
// if (countpodao1flag < 10) countpodao1flag = countpodao1flag + 1;
// }
// int poendleft = 0;
// int poendright = 0;
// int maxpoend = 0;
// if (turepodaoflag == 1 && countpodao1flag <10 && truepodaotype1 == 1 && truepodaotype2 == 0)
// {
// if (beginleft != 0)
// {
//
// for (i = beginleft; i <=50; i++)
// {
// if (My_Abs(L_black[i], L_black[i + 1]) > 5)
// {
// poendleft = i;
//
// break;
// }
// }
// }
//
// if (beginright != 0)
// {
//
// for (i = beginright; i <= 50; i++)
// {
// if (My_Abs(R_black[i], R_black[i + 1]) > 5)
// {
// poendright = i;
//
// break;
// }
// }
// }
///*
//在这十帧图像的过程中，可能有效赛道的行数退到了只有20-30行，需要将底部赛道向上方延续，
//一直补到69行。
//*/
//
// if (poendleft != 0 && poendright == 0)
// {
// maxpoend = poendleft;
// }
// if (poendleft == 0 && poendright != 0)
// {
// maxpoend = poendright;
// }
// if (poendleft != 0 && poendright != 0)
// {
// if(poendleft>= poendright) maxpoend = poendleft;
// if (poendleft < poendright) maxpoend = poendright;
// }
//
// if (maxpoend >=3&&maxpoend <= 50 && maxpoend != 0)
// {
// for (i = maxpoend - 2; i <= 69; i++)
// {
// L_black[i] = L_black[i - 1];
// R_black[i] = R_black[i - 1];
// LCenter[i] = (unsigned char)((unsigned char)L_black[i] / 2 + (unsigned char)R_black[i] / 2);
// }
// }
// }
// int ponobomp = 0;
// /*
// ponobomp=1 的意思是每一行的左右线保持着基本的连续状态(跳变比较小，没有杂乱无章的去跳变)
//
//
// */
// if (turepodaoflag == 1 && truepodaotype1 == 1 && truepodaotype2 == 0)
// {
// if (L_black[10] != 186 && R_black[10] != 0 && L_black[15] != 186 && R_black[15] != 0 && L_black[20] != 186 && R_black[20] != 0
// && L_black[13] != 186 && R_black[13] != 0 && L_black[18] != 186 && R_black[18] != 0
// && My_Abs(LCenter[0], LCenter[1]) <= 5 & My_Abs(LCenter[1], LCenter[2]) <= 5 & My_Abs(LCenter[2], LCenter[3]) <= 5 & My_Abs(LCenter[3], LCenter[4]) <= 5
// & My_Abs(LCenter[4], LCenter[5]) <= 5 & My_Abs(LCenter[5], LCenter[6]) <= 5 & My_Abs(LCenter[6], LCenter[7]) <= 5 & My_Abs(LCenter[7], LCenter[8]) <= 5
// & My_Abs(LCenter[8], LCenter[9]) <= 5 && My_Abs(LCenter[0], LCenter[9]) <= 10 && (calkuan[30] - calkuan[10] <= -10 || break_hangshu < 30)
// ) ponobomp = 1;
// }
///*
//计数满十帧之后 开始进入坡道的操作状态
//十帧初始 还未到坡道顶端 左边放在186 右线放在0 中线93 相当于锁定了舵机
//*/
// if (turepodaoflag == 1 && truepodaotype1 == 1 && truepodaotype2 == 0 && countpodao1flag == 10&& countpodao2flag<=20)
// {
//
// for (i = 0; i <= 60; i++)
// {
// L_black[i] = 186;
// LCenter[i] = 93;
// R_black[i] = 0;
// zhongold = 93;
// }
// countpodao2flag = countpodao2flag + 1;
// saoxianflag = 1;
// }
//
//
// if (saoxianflag == 1&& ((podaotype==2&&countpodao2flag> 15)|| (podaotype == 1 && countpodao2flag > 15)) && truepodaotype1 == 1 && truepodaotype2 == 0&&Pixels[0][93]==0
// && Pixels[0][ 92] == 0 && Pixels[0][ 91] == 0
// )
// {
// int leftstart = 0;
// int rightstart = 0;
// int leftend = 0;
// int rightend = 0;
// int lie;
// for (lie = 93; lie >= 1; lie--)
// {
// if (Pixels[0][ lie - 1] == 255 && Pixels[0][ lie] == 0 && Pixels[0][ lie + 1] == 0) //黑黑白 右边开始
// {
// rightstart = lie - 1;
//
// break;
// }
// }
// if (rightstart != 0)
// {
// for (lie = rightstart; lie > 1; lie--)
// {
// if (Pixels[0][ lie - 1] == 0 && Pixels[0][ lie] == 255 && Pixels[0][ lie + 1] == 255) //白白黑 右边结束
// {
// rightend = lie;
//
// break;
// }
// }
//
// }
// for (lie = 93; lie <= 184; lie++)
// {
// if (Pixels[0][ lie - 1] == 0 && Pixels[0][lie] == 0 && Pixels[0][lie + 1] == 255) //黑黑白 左边开始
// {
// leftstart = lie + 1;
//
// break;
// }
// }
// if (leftstart != 0)
// {
// for (lie = leftstart; lie <=184; lie++)
// {
// if (Pixels[0][ lie - 1] == 255 && Pixels[0][lie] == 255 && Pixels[0][lie + 1] == 0) //白白黑 左边结束
// {
// leftend = lie;
//
// break;
// }
// }
//
// }
// int widthright = rightstart - rightend;
// int widthleft = leftend - leftstart;
// zhongold = 93;
// if (widthright >= widthleft && widthright > 10)
// {
// zhongold = (rightstart + rightend) / 2;
// }
// if (widthright < widthleft && widthleft > 10)
// {
// zhongold = (leftstart + leftend) / 2;
// }
//
// }
//
// // SetText("zhongold " + zhongold);
// // SetText("zhongold " + zhongold);
// // SetText("zhongold " + zhongold);
// //if (turepodaoflag == 1 && truepodaotype1 ==1&& truepodaotype2 == 0&& ((kuandubreak != 0 && kuandubreak <= 65 && kuandubreakhang >= 32 && end > 30) || (kuandubreak != 0 && kuandubreak <= 85 && kuandubreakhang < 38 && kuandubreakhang >= 22 && end > 22))
// // && (kerrorL <= 0.6||kerrorR <= 0.6)&&calkuan[0]<170&& countpodao1flag==10 && calkuan[0] >30
// // &&(continueleft==1||continueright==1)&&break_hangshu>=25
// // )//((kuandubreak != 0 && kuandubreak <= 65 &&kuandubreakhang>= 32 && end > 30) || (kuandubreak != 0 && kuandubreak <= 85 && kuandubreakhang<38&& kuandubreakhang>=22 && end > 22))
// if (turepodaoflag == 1 && truepodaotype1 == 1 && truepodaotype2 == 0 && (continueleft == 1 || continueright == 1) && break_hangshu >= 25 && countpodao1flag == 10
// && (calkuan[0] > 40 && calkuan[0] < 150) && (calkuan[3] > 40 && calkuan[3] < 150) && (calkuan[6] > 40 && calkuan[6] < 150) && (calkuan[9] > 40 && calkuan[9] < 150)
// && (calkuan[12] > 40 && calkuan[12] < 150) && (calkuan[14] > 40 && calkuan[14] < 150) && (calkuan[16] > 40 && calkuan[16] < 150) && (calkuan[18] > 40 && calkuan[18] < 150)
// && ponobomp==1
//
// )
// {
//
// saoxianflag = 0;
// countpodao1flag = 0;
// turepodaoflag = 1;
// truepodaotype1 = 0;
// truepodaotype2 =1 ;
// podaotype2 = 0;
// countpodao2flag = 0;
// podaotype = 0;
//
// }
//
// if (turepodaoflag == 1 && truepodaotype1 == 0 && truepodaotype2 == 1 &&
// (break_hangshu < 43||(break_hangshu > 43&&(L_black[43] - R_black[43] < 50 || L_black[44] - R_black[44] < 50 || L_black[45] - R_black[45] < 50 || L_black[46]-R_black[46]<50|| L_black[47] - R_black[47] < 50 || L_black[48] - R_black[48] < 50 || L_black[49] - R_black[49] < 50 || L_black[50] - R_black[50] < 50)))
// &&( calkuan[0] > 165||(L_black[0]>=185&&R_black[0]>=30)
// || (L_black[0] <=155 && R_black[0] <=1)
// )&& jishupodaotype != 4)
// {
//
// turepodaoflag = 0;
// truepodaotype1 = 0;
// truepodaotype2 = 0;
// saoxianflag = 0;
// countpodao1flag = 0;
// countpodao2flag = 0;
//
//
// }
//
// if (turepodaoflag == 1 && truepodaotype1 == 0 && truepodaotype2 == 1 &&
// (break_hangshu < 44)
// && (calkuan[0] > 165 || (L_black[0] >= 185 && R_black[0] >= 30)
// || (L_black[0] <= 155 && R_black[0] <= 1)
// )&&jishupodaotype==4)
// {
//
// turepodaoflag = 0;
// truepodaotype1 = 0;
// truepodaotype2 = 0;
// saoxianflag = 0;
// countpodao1flag = 0;
// countpodao2flag = 0;
//
//
// }
//
// //if (turepodaoflag == 1 && podaotype2 == 1 && break_hangshu < 30 && break_hangshu > 20 && (L_black[10] - R_black[10]) < 70 && midddlecontinue == 1)
// //{
// // turepodaoflag = 0;
// // podaotype2 = 0;
// //}
// }
// if (turepodaoflag == 0 && lasttruepodaoflag == 1)
// { countpodao = 1; }
//
//
//}
////====================================坡道结束================================================
//
//
//=====================================车库====================================================后期调
///////////////////////////////////////有问题
/*
车库核心思路讲解:
车库的判别分为两大部分 第一部分是判别元素中存在车库 第二部分是特殊的扫线模式
第一部分讲解: 判别是否存在车库 主要是这种语句
if (Pixels[i][j] == 0 && Pixels[i][ j + 1] == 0 && Pixels[i][ j + 2] == 1 && Pixels[i][ j + 3] == 1)
寻找的是斑马线的黑白跳变，当存在基础的黑白跳变之后 我们要寻找宽度的变化
宽度的变化:车库因为有斑马线的存在，所以宽度的体现会是宽-窄-宽 ，其主要语句是(注意这里的big-smal-big)
for (i = 1; i < break_hangshu; i++)
{
if (calkuan[i] > 90)
{
kuandubig1 = i;
// SetText("equal第一个 " + i);
break;
}
}
if (kuandubig1 != 0)
{
for (i = kuandubig1; i < break_hangshu; i++)
{
if (calkuan[i] <= 65&&L_black[i]!=186&&R_black[i]!=0)
{
kuandusmall1 = i;
// SetText("equal第二个 " + i);
break;
}
}
}
if (kuandusmall1 != 0)
{
for (i = kuandusmall1; i < break_hangshu; i++)
{
if (calkuan[i] >= 76&& calkuan[i] <=150)
{
kuandubig2 = i;
// SetText("equal第三个 " + i);
break;
}
}
}
好的，如果你上面两步都理解完了，下面我们进入车库的特殊扫线模式。
首先要指出的当你出库的时候，你是知道将来库的方向的。比如常规跑法，右边出库，那么将来车库一定是在右边。
当满足上方两个条件(黑白跳变,宽度变化)的时候，假设是右方车库,有下面的操作:
(1)单独去扫右边线，从93往右扫线，扫到第一行的右线之后，以此为基准把右边线都扫出来。(方法是行数增加一行，列数往右退大约5列然后再去向左扫线。)
(2)右边线完整之后，我们通过从右线往左线扫线获得基本完整的左边线。
(3)完整的右线和有空行的左边线都扫出来之后，下面就是找左边线的上下拐点拉线，然后左加右除以二计算中线。如果不入库，就判长直道冲刺。如果入库拉线即可。
(4)关键是将有效的左右线扫出来，下面找拐点拉线的操作我觉得大家都已经写到车库元素了，很容易拿下。
*/
void mynewleftku()
{
int i;
int j;
int kuandubig1 = 0;
int kuandusmall1 = 0;
int kuandubig2 = 0;
int kukuanduequal = 0;
int leftku1 = 0;
int leftku1next = 0;
int leftku2 = 0;
int leftku2next = 0;
int leftku3 = 0;
int leftku3next = 0;
int leftku4 = 0;
int leftku4next = 0;
int leftku5 = 0;
int leftku5next = 0;
//threemode=12; //暂时加上 千万注意
for (i = 1; i < break_hangshu; i++)
{
if (calkuan[i] > 90)///////数值可能要改
{
kuandubig1 = i;
// SetText("equal第一个 " + i);
break;
}
}
if (kuandubig1 != 0)
{
for (i = kuandubig1; i < break_hangshu; i++)
{
if (calkuan[i] <= 55&&L_black[i]!=186&&R_black[i]!=0)/////数值可能要改
{
kuandusmall1 = i;
// SetText("equal第二个 " + i);
break;
}
}
}
if (kuandusmall1 != 0)
{
for (i = kuandusmall1; i < break_hangshu; i++)
{
if (calkuan[i] >= 72&& calkuan[i] <=150)///////数值可能也要改
{
kuandubig2 = i;
// SetText("equal第三个 " + i);
break;
}
}
}
if (kuandubig1 != 0 && kuandusmall1 != 0 && kuandubig2 != 0 && turepodaoflag == 0 && huandao_memory == 0)
{
kukuanduequal = 1;
}
firstku = 0;
kurightdownflag = 0; //初步找一遍
kuleftdownflag = 0;
kurightupflag = 0;
kuleftupflag = 0;
find_kurightdown_point(5, 50);
find_kuleftdown_point(5, 50);
find_kuleftup_point(3, 55);///////////////////////
find_kurightup_point(3, 55);//////////////////////
// if (kukuanduequal == 1 && (kurightdownflag == 1|| kurightupflag==1) && (kuleftdownflag == 1|| kuleftupflag==1))
// {
// kumode = 1;
// // SetText("kumode " + kumode);
// }
if (kukuanduequal == 1 && (kurightdownflag == 1 && kurightupflag == 1))
{
kumode = 1;
// SetText("kumode " + kumode);
}
if (kukuanduequal == 1 && (kuleftdownflag == 1 && kuleftupflag == 1))
{
kumode = 2;//////////////////////////////////////////感觉bug
//SetText("kumode " + kumode);
}
///////////////////////////////////////////////////////////////////////////////////////////////////
// if ((threemode == 6||threemode==12) && type == 4 && kukuanduequal == 1 &&My_Abs( pianfangright ,pianfangleft )>= 230 && kurightdownflag == 1 && kuleftdownflag == 0&& kuright_turn_down[1]<=150
// && turepodaoflag == 0 && huandao_memory == 0 && huandao_memoryforleft == 0 && firstmid == 0 && secondmid == 0&& (continueleft == 0 || continueright == 0) && break_hangshu <= 40
// )
// {
// // wankuflag = 1;
// // truewankuflag=1;
// }
// if (wankuflag == 1)
// {
// if (R_black[0] != 0 && R_black[1] != 0 && R_black[2] != 0 && R_black[3] != 0 && R_black[4] != 0 && R_black[5] != 0)
// {
// advanced_regression(2, 1, 3, 4, 5);
// R_blackfuzhi(parameterA, parameterB, 0, 50);
// }
// else
// {
// for (i = 0; i < 50; i++)
// {
// R_black[i] = 0;
// }
// }
// if(L_black[0]!=186&& L_black[1] != 186 && L_black[2] != 186 && L_black[3] != 186 && L_black[4] != 186 && L_black[5] != 186)
// {
// advanced_regression(1, 1, 3, 4, 5);
// leftlinefuzhi(parameterA, parameterB, 0, 50);
// }
// else
// {
// for (i = 0; i < 50; i++)
// {
// L_black[i] = 186;
// }
// }
// zhongxianfuzhi(0, 50);
// kumode = 1;
// }/////////////////////////////////////////////////////////////////////////////没用
// if ((threemode == 6 || threemode == 12) && type == 3 && kukuanduequal == 1 && My_Abs(pianfangright, pianfangleft) >= 230 && kurightdownflag == 0 && kuleftdownflag == 1 && kuleft_turn_down[1] >=5
// && turepodaoflag == 0 && huandao_memory == 0 && huandao_memoryforleft == 0 && firstmid == 0 && secondmid == 0&& (continueleft == 0 || continueright == 0) && break_hangshu <= 40
// )
// {
// // wankuflag = 2;
// // truewankuflag=1;
// }
// if (wankuflag == 2)
// {
// if (R_black[0] != 0 && R_black[1] != 0 && R_black[2] != 0 && R_black[3] != 0 && R_black[4] != 0 && R_black[5] != 0)
// {
// advanced_regression(2, 1, 3, 4, 5);
// R_blackfuzhi(parameterA, parameterB, 0, 50);
// }
// else
// {
// for (i = 0; i < 50; i++)
// {
// R_black[i] = 0;
// }
// }
// if (L_black[0] != 186 && L_black[1] != 186 && L_black[2] != 186 && L_black[3] != 186 && L_black[4] != 186 && L_black[5] != 186)
// {
// advanced_regression(1, 1, 3, 4, 5);
// leftlinefuzhi(parameterA, parameterB, 0, 50);
// }
// else
// {
// for (i = 0; i < 50; i++)
// {
// L_black[i] = 186;
// }
// }
// zhongxianfuzhi(0, 50);
// kumode = 1;
// }//////////////////////////////////////////////////////////////////////////////////////没用
if (kumode == 1||kumode==2) //基本模式试着扫斑马线
{
recordhang = horizon;
haveleftku = 0;
leftkuhang = 0;
for (i = 0; i < recordhang; i++)
{
leftku1 = 0;
leftku1next = 0;
leftku2 = 0;
leftku2next = 0;
leftku3 = 0;
leftku3next = 0;
leftku4 = 0;
leftku4next = 0;
leftku5 = 0;
leftku5next = 0;
for (j = 30; j < 150; j++)
{
if (Pixels[i][j] == 0 && Pixels[i][ j + 1] == 0 && Pixels[i][ j + 2] == 1 && Pixels[i][ j + 3] == 1)
{
leftku1 = 1;
leftku1next = j + 4;
break;
}
}
if (leftku1next != 0)
{
for (j = leftku1next; j < 150; j++)
{
if (Pixels[i][ j] == 0 && Pixels[i][ j + 1] == 0 && Pixels[i][ j + 2] == 1 && Pixels[i][ j + 3] == 1)
{
leftku2 = 1;
leftku2next = j + 4;
break;
}
}
}
if (leftku2next != 0)
{
for (j = leftku2next; j < 150; j++)
{
if (Pixels[i][ j] == 0 && Pixels[i][ j + 1] == 0 && Pixels[i][ j + 2] == 1 && Pixels[i][ j + 3] == 1)
{
leftku3 = 1;
leftku3next = j + 4;
break;
}
}
}
if (leftku3next != 0)
{
for (j = leftku3next; j < 150; j++)
{
if (Pixels[i][ j] == 0 && Pixels[i][ j + 1] == 0 && Pixels[i][j + 2] == 1 && Pixels[i][ j + 3] == 1)
{
leftku4 = 1;
leftku4next = j + 4;
break;
}
}
}
if (leftku4next != 0)
{
for (j = leftku4next; j < 150; j++)
{
if (Pixels[i][j] == 0 && Pixels[i][j + 1] == 0 && Pixels[i][ j + 2] == 1 && Pixels[i][j + 3] == 1)
{
leftku5 = 1;
leftku5next = j + 4;
break;
}
}
}
if (leftku1 == 1 && leftku2 == 1 && leftku3 == 1 && leftku4 == 1 && leftku5 == 1)
{
shifouyoubanmaxian=1;
haveleftku = 1;
leftkuhang = i;
recordhang = i + 7;
break;
}
}
}
if (haveleftku == 1 && leftkuhang != 0)
{
youku=1;
if (huandao_memory != 0) { huandao_memory = 0; huandaojishu = huandaojishu - 1; }
if (huandao_memoryforleft != 0) { huandao_memoryforleft = 0; huandaojishu = huandaojishu - 1; }
}
// if (kumode==2)
// {
// wankuflag = 0;
// truewankuflag=0;
// }
// if (kumode == 2)
// {
// wankuflag = 0;
// truewankuflag=0;
// }
if (haveleftku == 0)
{
kumode = 0;
}
//type = 3;
// type = 4;
int hang, lie;
//重新扫线左出左入开始
if (kumode == 2 && youku==1)
{
for (lie = 0; lie <184; lie++)
{
if (lie >= (int)184) lie = 183;
if ( Pixels[0][ lie] == 0 && Pixels[0][ lie + 1] == 1 && Pixels[0][ lie + 2] == 1) //黑白跳变
{
R_black[0] = (unsigned char)(lie + 1);
rightold = (int)(lie + 1);
rightflag[0] = 1;
break;
}
else
{
R_black[0] = 0;
rightflag[0] = 0;
}
}
for (hang = 1; hang < horizon; hang++) //1-60行扫右线
{
for (lie = R_black[hang - 1] - 2; lie < 184; lie++)
{
if (lie <= 0) lie = 2;
if (lie >= (int)184) lie = 183;
if (Pixels[hang][ lie] == 0 && Pixels[hang][ lie + 1] == 1 && Pixels[hang][ lie + 2] == 1) //黑白
{
R_black[hang] = (unsigned char)(lie + 1);
rightold = (int)(lie + 1);
rightflag[hang] = 1;
if(R_black[hang]==4)
{
R_black[hang] = 0;
rightflag[hang] = 0;
}
break;
}
else
{
R_black[hang] = 0;
rightflag[hang] = 0;
}
}
}
for (lie = 188; lie >0; lie--)
{
if ( Pixels[0][ lie] == 0 && Pixels[0][ lie - 1] == 1 && Pixels[0][ lie - 2] == 1) //黑白跳变
{
L_black[0] = (unsigned char)(lie - 1);
leftold = (int)(lie - 1);
leftflag[0] = 1;
break;
}
else
{
L_black[0] = 186;
leftflag[0] = 0;
}
}
for (hang = 1; hang < horizon; hang++) //1-60行扫右线
{
for (lie = L_black[hang - 1] + 2; lie > 0; lie--)
{
if (Pixels[hang][ lie] == 0 && Pixels[hang][ lie - 1] == 1 && Pixels[hang][ lie - 2] == 1) //黑白
{
L_black[hang] = (unsigned char)(lie - 1);
leftold = (int)(lie - 1);
leftflag[hang] = 1;
break;
}
else
{
R_black[hang] = 0;
rightflag[hang] = 0;
}
}
}
}
if (kumode == 2 &&youku==1)
{
firstku = 0;
kuleftdownflag = 0;
find_kuleftdown_point(5, horizon);
if (kuleftdownflag == 1)
{
kuleftupflag = 0;
if (kuleft_turn_down[0] >= 3 && kuleft_turn_down[0] <= horizon)
{ find_kuleftup_point(kuleft_turn_down[0], horizon); }
else
{
find_kuleftup_point(3, horizon);
}
}
if (kuleftdownflag == 0)
{
kuleftupflag = 0;
find_kuleftup_point(3, horizon);
}
if (kuleftdownflag == 1 && kuleftupflag == 1&& kuleft_turn_down[0]>=5)
{
advanced_regression(1, kuleft_turn_down[0] - 5, kuleft_turn_down[0] - 3, kuleft_turn_up[0] + 2, kuleft_turn_up[0] + 4);
leftlinefuzhi(parameterA, parameterB, 0, horizon);
int rightmax=0;
for (i = kuleft_turn_up[0] + 2; i >= 1; i--)
{
if (My_Abs(R_black[i], R_black[i - 1]) >= 5)
{
rightmax = i;
break;
}
}
if (rightmax != 0)
{
advanced_regression(2, rightmax+1, rightmax+2, rightmax+4, rightmax+5);
R_blackfuzhi(parameterA, parameterB, 0, rightmax + 5);
}
firstku = 1;
zhongxianfuzhi(0, horizon);
}
if (kuleftdownflag == 0 && kuleftupflag == 1)
{
advanced_regression(1, kuleft_turn_up[0]+1, kuleft_turn_up[0]+2, kuleft_turn_up[0] +4, kuleft_turn_up[0] + 5);
leftlinefuzhi(parameterA, parameterB, 0, horizon);
int rightmax = 0;
for (i = kuleft_turn_up[0] + 2; i >= 1; i--)
{
if (My_Abs(R_black[i], R_black[i - 1]) >= 5)
{
rightmax = i;
break;
}
}
if (rightmax != 0)
{
advanced_regression(2, rightmax + 1, rightmax + 2, rightmax + 4, rightmax + 5);
R_blackfuzhi(parameterA, parameterB, 0, rightmax + 5);
}
firstku = 1;
zhongxianfuzhi(0, horizon);
}
}
if (kumode == 2 && type == 3&&rukuflag!=3&& threemode == 12&&youku==1)
{
firstku = 0;
kuleftdownflag = 0;
find_kuleftdown_point(5, 40);
if (kuleftdownflag == 1)
{
kuleftupflag = 0;
if (kuleft_turn_down[0] >= 3 && kuleft_turn_down[0] <= 39)
{ find_kuleftup_point(kuleft_turn_down[0], horizon); }
else
{
find_kuleftup_point(3, horizon);
}
}
if (kuleftdownflag == 0)
{
kuleftupflag = 0;
find_kuleftup_point(3, horizon);
}
if (kuleftdownflag == 1 && kuleftupflag == 1 && kuleft_turn_up[0] >= 21&& kuleft_turn_down[0]>=5)
{
advanced_regression(1, kuleft_turn_down[0] - 5, kuleft_turn_down[0] - 3, kuleft_turn_up[0] + 2, kuleft_turn_up[0] + 4);
leftlinefuzhi(parameterA, parameterB, 0, horizon);
int rightmax = 0;
for (i = kuleft_turn_up[0] + 2; i >= 1; i--)
{
if (My_Abs(R_black[i], R_black[i - 1]) >= 5)
{
rightmax = i;
break;
}
}
if (rightmax != 0)
{
advanced_regression(2, rightmax + 1, rightmax + 2, rightmax + 4, rightmax + 5);
R_blackfuzhi(parameterA, parameterB, 0, rightmax + 5);
}
zhongxianfuzhi(0, horizon);
if (kuleft_turn_up[0] > 35) rukuflag = 1;
if (kuleft_turn_up[0]>=30&& kuleft_turn_up[0]<=35) rukuflag = 2;
}
if (kuleftdownflag == 0 && kuleftupflag == 1&& kuleft_turn_up[0] >= 21)
{
advanced_regression(1, kuleft_turn_up[0] + 1, kuleft_turn_up[0] + 2, kuleft_turn_up[0] + 4, kuleft_turn_up[0] + 5);
leftlinefuzhi(parameterA, parameterB, 0, horizon);
int rightmax = 0;
for (i = kuleft_turn_up[0] + 2; i >= 1; i--)
{
if (My_Abs(R_black[i], R_black[i - 1]) >= 5)
{
rightmax = i;
break;
}
}
if (rightmax != 0)
{
advanced_regression(2, rightmax + 1, rightmax + 2, rightmax + 4, rightmax + 5);
R_blackfuzhi(parameterA, parameterB, 0, rightmax + 5);
}
zhongxianfuzhi(0, 50);
if (kuleft_turn_up[0] > 35) rukuflag = 1;
if (kuleft_turn_up[0] >= 30 && kuleft_turn_up[0] <= 35) rukuflag = 2;
}
if ((kuleftupflag == 1 && kuleft_turn_up[0] < 21)|| leftkuhang <= 11)
{
for (i = 0; i < 50; i++)
{
L_black[i] = 186;
}
R_black[kuleft_turn_up[0]] = (unsigned char)kuleft_turn_up[1];
R_black[kuleft_turn_up[0]+1] = (unsigned char)(kuleft_turn_up[1]+3);
R_black[0] = (unsigned char)0;
R_black[2] = (unsigned char)3;
advanced_regression(2, 0, 2, kuleft_turn_up[0], kuleft_turn_up[0] + 1);
R_blackfuzhi(parameterA, parameterB, 0, 50);
zhongxianfuzhi(0, 50);
rukuflag = 3;
for (i = 0; i < 50; i++)
{
LCenterrecord[i] = LCenter[i];
rightrecord[i] = R_black[i];
leftrecord[i] = L_black[i];
}
}
}
//重新扫线左出左入结束
//重新扫线右出右出开始
if (kumode == 1 &&youku==1)
{
for (lie = 185; lie >=2; lie--)
{
if ( Pixels[0][ lie] == 0 && Pixels[0][ lie - 1] == 1 && Pixels[0][ lie - 2] == 1)
{
L_black[0] = (unsigned char)(lie - 1);
leftold = (int)(lie - 1);
leftflag[0] = 1;
break;
}
else
{
L_black[0] = 186;
leftflag[0] = 0;
}
}
for (hang = 1; hang < 50; hang++) //1-70行扫左线
{
for (lie = L_black[hang-1]+2; lie >=1; lie--) //扫左线
{
if (lie <= 1) lie = 1;
if (lie >= 184) lie = 184;
if (Pixels[hang][ lie + 1] == 0 && Pixels[hang][ lie] == 0 && Pixels[hang][ lie - 1] == 1)
{
L_black[hang] = (unsigned char)(lie - 1);
leftold = (int)(lie - 1);
leftflag[hang] = 1;
break;
}
else
{
L_black[hang] = 186;
leftflag[hang] = 0;
}
}
}
for (hang = 0; hang < 50; hang++) //扫右线
{
for (lie = L_black[hang]; lie >= 1; lie--)
{
if (lie >= (int)185) lie = 184;
if (Pixels[hang][lie +1] == 1 && Pixels[hang][ lie] == 1 && Pixels[hang][lie - 1] == 0) //黑白
{
R_black[hang] = (unsigned char)(lie);
rightold = (int16)(lie);
rightflag[hang] = 1;
break;
}
else
{
R_black[hang] = 0;
rightflag[hang] = 0;
}
}
}
}
if (kumode == 1 && type == 4 && threemode == 6&&youku==1)//&& threemode == 6
{
firstku = 0;
kurightdownflag = 0;
find_kurightdown_point(5, 35);
if (kurightdownflag == 1)
{
kurightupflag = 0;
if (kuleft_turn_down[0] <= 39&& kuleft_turn_down[0]>=3)
{ find_kurightup_point(kuleft_turn_down[0], 40); }
else
{ find_kurightup_point(3, 40); }
}
if (kurightdownflag == 0)
{
kurightupflag = 0;
find_kurightup_point(3, 40);
}
if (kurightdownflag == 1 && kurightupflag == 1 && kuright_turn_down[0] >= 5) //
{
advanced_regression(2, kuright_turn_down[0] - 5, kuright_turn_down[0] - 3, kuright_turn_up[0] + 2, kuright_turn_up[0] + 4);
R_blackfuzhi(parameterA, parameterB, 0, 50);
int leftmax = 0;
for (i = kuright_turn_up[0] + 2; i >= 1; i--)
{
if (My_Abs(L_black[i], L_black[i - 1]) >= 5)
{
leftmax = i;
break;
}
}
if (leftmax != 0)
{
advanced_regression(1, leftmax + 1, leftmax + 2, leftmax + 4, leftmax + 5);
leftlinefuzhi(parameterA, parameterB, 0, leftmax + 5);
}
zhongxianfuzhi(0, 50);
firstku = 1;
}
if (kurightdownflag == 0 && kurightupflag == 1)
{
advanced_regression(2, kuright_turn_up[0] + 1, kuright_turn_up[0] + 2, kuright_turn_up[0] + 4, kuright_turn_up[0] + 5);
R_blackfuzhi(parameterA, parameterB, 0, 50);
int leftmax = 0;
for (i = kuleft_turn_up[0] + 2; i >= 1; i--)
{
if (My_Abs(L_black[i], L_black[i - 1]) >= 5)
{
leftmax = i;
break;
}
}
if (leftmax != 0)
{
advanced_regression(1, leftmax + 1, leftmax + 2, leftmax + 4, leftmax + 5);
leftlinefuzhi(parameterA, parameterB, 0, leftmax + 5);
}
zhongxianfuzhi(0, 50);
firstku = 1;
}
}
if (kumode == 1 && rukuflag != 3 && threemode == 12&&youku==1) //
{
firstku = 0;
kurightdownflag = 0;
find_kurightdown_point(5, horizon);
if (kurightdownflag == 1)
{
kurightupflag = 0;
if (kuleft_turn_down[0] <= 39&& kuleft_turn_down[0]>=3)
{ find_kurightup_point(kuleft_turn_down[0], 40); }
else
{ find_kurightup_point(3, 40); }
}
if (kurightdownflag == 0)
{
kurightupflag = 0;
find_kurightup_point(3, 40);
}
if (kurightdownflag == 1 && kurightupflag == 1 && kuright_turn_down[0] >= 5 && kuright_turn_up[0] >= 21) //
{
advanced_regression(2, kuright_turn_down[0] - 5, kuright_turn_down[0] - 3, kuright_turn_up[0] + 2, kuright_turn_up[0] + 4);
R_blackfuzhi(parameterA, parameterB, 0, 50);
int leftmax = 0;
for (i = kuright_turn_up[0] + 2; i >= 1; i--)
{
if (My_Abs(L_black[i], L_black[i - 1]) >= 5)
{
leftmax = i;
break;
}
}
if (leftmax != 0)
{
advanced_regression(1, leftmax + 1, leftmax + 2, leftmax + 4, leftmax + 5);
leftlinefuzhi(parameterA, parameterB, 0, leftmax + 5);
}
zhongxianfuzhi(0, 50);
if (kuright_turn_up[0] > 35) rukuflag = 1;
if (kuright_turn_up[0] >= 30 && kuright_turn_up[0] <= 35) rukuflag = 2;
}
if (kurightdownflag == 0 && kurightupflag == 1 && kuright_turn_up[0] >= 21)
{
advanced_regression(2, kuright_turn_up[0] + 1, kuright_turn_up[0] + 2, kuright_turn_up[0] + 4, kuright_turn_up[0] + 5);
R_blackfuzhi(parameterA, parameterB, 0, 50);
int leftmax = 0;
for (i = kuleft_turn_up[0] + 2; i >= 1; i--)
{
if (My_Abs(L_black[i], L_black[i - 1]) >= 5)
{
leftmax = i;
break;
}
}
if (leftmax != 0)
{
advanced_regression(1, leftmax + 1, leftmax + 2, leftmax + 4, leftmax + 5);
leftlinefuzhi(parameterA, parameterB, 0, leftmax + 5);
}
zhongxianfuzhi(0, 50);
if (kuright_turn_up[0] > 35) rukuflag = 1;
if (kuright_turn_up[0] >= 30 && kuright_turn_up[0] <= 35) rukuflag = 2;
}
if ( (kurightupflag == 1 && kuright_turn_up[0]< 21)||leftkuhang<=11)
{
for (i = 0; i < 50; i++)
{
R_black[i] = 0;
}
L_black[kuright_turn_up[0]] = (unsigned char)kuright_turn_up[1];
L_black[kuright_turn_up[0] + 1] = (unsigned char)(kuright_turn_up[1]-3);
L_black[0] = (unsigned char)186;
L_black[2] = (unsigned char)183;
advanced_regression(1, 0, 2, kuright_turn_up[0], kuright_turn_up[0] + 1);
leftlinefuzhi(parameterA, parameterB, 0, 50);
zhongxianfuzhi(0, 50);
rukuflag = 3;
for (i = 0; i < 50; i++)
{
LCenterrecord[i] = LCenter[i];
rightrecord[i] = R_black[i];
leftrecord[i] = L_black[i];
}
}
}
//重新扫线右出右入结束
if (rukuflag == 3)
{
for (i = 0; i < 50; i++)
{
LCenter[i] = (unsigned char)LCenterrecord[i];
R_black[i] = (unsigned char)rightrecord[i];
L_black[i] = (unsigned char)leftrecord[i];
}
}
if (countfirstku != 0) countfirstku = countfirstku + 1; //第一次入库保护
if (countfirstku == 5) countfirstku = 0;
if (firstku == 0 && lastfirstku == 1) countfirstku = 1;
lastfirstku = firstku;
}
//=====================================车库=====================================================
//=====================================车库辅助判断===============================================
void find_kurightdown_point(int start_point, int end_point)//得到拐点的行与列 int
{
int j;
if (end_point >= break_hangshu) end_point = break_hangshu - 2;
for (j = start_point; j <= end_point; j++)
{
if (My_Abs(R_black[j - 3], R_black[j - 2]) <= 3 && My_Abs(R_black[j - 2], R_black[j - 1]) <= 3 && My_Abs(R_black[j - 1], R_black[j]) <= 3 && My_Abs(R_black[j], R_black[j + 2]) >= 6&&My_Abs(R_black[j], R_black[j + 1]) >= 6 && ((L_black[j]-R_black[j]>40&&j<25)||j>=25)&&
(R_black[j+1]<=10|| R_black[j + 2] <= 10 || R_black[j + 3] <= 10 || R_black[j + 4] <= 10|| (My_Abs(R_black[j], R_black[j+4])>=30&& R_black[j + 4]!=0))
&& rightflag[j - 3] == 1 && rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1) //样本为4行，图变差据
{
int record = 0;
int i;
record = j;
for (i = j; i >= 1; i--)
{
if (R_black[i] == R_black[i - 1]) record = i - 1;
if (R_black[i] != R_black[i - 1]) break;
}
kuright_turn_down[0] = (int)(record);//数组里面没有第0行
kuright_turn_down[1] = R_black[record];
kurightdownflag = 1;
break; //记住具体的行与列
}
}
}
void find_kuleftdown_point(int start_point, int end_point)
{
int j;
if (end_point >= break_hangshu) end_point = break_hangshu - 2;
for (j = start_point; j <= end_point; j++)
{
if (My_Abs(L_black[j - 3], L_black[j - 2]) <= 3 && My_Abs(L_black[j - 2], L_black[j - 1]) <= 3 && My_Abs(L_black[j - 1], L_black[j]) <= 3 && (My_Abs(L_black[j], L_black[j + 2]) >= 5&&(My_Abs(L_black[j], L_black[j + 1]) >= 6||(My_Abs(L_black[j], L_black[j + 1]) >= 4 && L_black[j + 1]==186)) && ((L_black[j] - R_black[j] > 40 && j < 25) || j >= 25)
&& (L_black[j + 1] >=175 || L_black[j + 2] >= 175 || L_black[j + 3] >= 175 || L_black[j + 4] >= 175 || (My_Abs(L_black[j ], L_black[j+4])>=30&& L_black[j + 4]!=0))
&& leftflag[j - 3] == 1 && leftflag[j - 2] == 1 && leftflag[j - 1] == 1 && leftflag[j] == 1&& L_black[j]>=60)) //样本为4行，图变差据
{
int record = 0;
int i;
record = j;
for (i = j; i >= 1; i--)
{
if (L_black[i] == L_black[i - 1]) record = i - 1;
if (L_black[i] != L_black[i - 1]) break;
}
kuleft_turn_down[0] = (int)(record);//数组里面没有第0行
kuleft_turn_down[1] = L_black[record]; //记住行与列
kuleftdownflag = 1;
break; //找到突变点之后就跳出
}
}
}
void find_kurightup_point(int start_point, int end_point)
{
int j;
//for (j = end_point; j >= start_point; j--)
for (j = start_point; j <= end_point; j++)
{
if (My_Abs(R_black[j + 2], R_black[j + 1]) <= 4 && My_Abs(R_black[j + 1], R_black[j]) <= 4 && My_Abs(R_black[j + 3], R_black[j + 2]) <= 4 && My_Abs(R_black[j], R_black[j - 1])&& My_Abs(R_black[j], R_black[j - 3]) >= 5 && R_black[j] <=93
&& rightflag[j] == 1 && rightflag[j + 1] == 1 && rightflag[j + 2] == 1 && rightflag[j + 3] == 1&&R_black[j]>=3) //样本为4行，图变差据
{
int record = 0;
int i;
record = j;
for (i = j; i >= 1; i--)
{
if (R_black[i] == R_black[i - 1]) record = i - 1;
if (R_black[i] != R_black[i - 1]) break;
}
kuright_turn_up[0] = (int)(record);//数组里面没有第0行
kuright_turn_up[1] = R_black[record]; //记住行与列
kurightupflag = 1;
break; //找到突变点之后就跳出
}
}
}
void find_kuleftup_point(int start_point, int end_point)
{
int j;
//for (j = end_point; j>= start_point; j--)
for (j = start_point; j <= end_point; j++)
{
if (My_Abs(L_black[j+2], L_black[j+1]) <= 4 && My_Abs(L_black[j+1], L_black[j]) <= 4 && My_Abs(L_black[j +3], L_black[j+2]) <= 4 && My_Abs(L_black[j], L_black[j -1]) >= 5&& My_Abs(L_black[j], L_black[j -3]) >= 5&&L_black[j]>=93
&& leftflag[j] == 1 && leftflag[j+1] == 1 && leftflag[j+2] == 1 && leftflag[j+3] == 1&&L_black[j]<=183) //样本为4行，图变差据
{
int record = 0;
int i;
record = j;
for (i = j; i >= 1; i--)
{
if (L_black[i] == L_black[i - 1]) record = i - 1;
if (L_black[i] != L_black[i - 1]) break;
}
kuleft_turn_up[0] = (int)(record);//数组里面没有第0行
kuleft_turn_up[1] = L_black[record]; //记住行与列
kuleftupflag = 1;
break; //找到突变点之后就跳出
}
}
}
void leftlinefuzhi(float A, float B, int8 start_point, int8 end_point)
{
int8 m;
for (m = start_point; m <= end_point; m++)
{
if((B * m + A)>=255) L_black[m]=255;
if((B * m + A)<=0) L_black[m]=0;
else if(0<(B * m + A)&&(B * m + A)<255)L_black[m] = (int8)(B * m + A);
}
}
void R_blackfuzhi(float A, float B, int8 start_point, int8 end_point)
{
int8 m;
for (m = start_point; m <= end_point; m++)
{
if ((B * m + A) < 0) {R_black[m] = 0; }
else if((B * m + A) >=0&&(B * m + A)<=255) R_black[m] = (int8)(B * m + A);
if((B * m + A)>=255) R_black[m]=255;
}
}
void zhongxianfuzhi( int8 start_point, int8 end_point)
{
int8 m;
for (m = start_point; m <= end_point; m++)
{
LCenter[m] = (int8)(L_black[m]/2 + R_black[m]/ 2);
}
}
//=====================================车库辅助判断=================================================
//========================================右环岛==================================================
/*
注释1:环岛的八个状态
环岛分为八个状态。以右环岛为例，一二三状态都在未入环的直道上，这时候需要把右环岛的
中拐点和下拐点拉起来，补成一条标准的直道。四状态在右环岛的入口那边，需要把右环岛的
上拐点和左边边线上的某一点连线，封住前方的道路，让车模进入圆环。五六状态都在环岛里
面跑，没有什么技巧。其状态就是到出环岛的口了，这时候需要把左下角的点和白色赛道的右
上角的点连起来，让车子右转。八状态车模基本上就已经正了，这个时候需要做的事把右环岛
中拐点和上拐点连起来，让车模跑直线。
*/
/*注释2:
各个变量的含义
huandaotype:环岛的类型，在跑之前先手动输入环岛大小 50 / 60... 来给出不同的判断
turepodaoflag :坡道标志位，即坡道上不判断
countpodao :坡道结束的帧数计数
rukuflag :车库标志位，在判断要进入车库时不判环岛
threemode :三叉模式标志位
firstku、countfirstku:第一次经过车库时标志位(因为我的车要跑两圈)
truedoubleright :找到右边线的双断点时置1
huandao_memory :环岛状态标志位
flag_find_huan_leftdown_point
flag_find_huan_rightdown_point
flag_find_huan_rightmiddle_point
flag_find_huan_leftmiddle_point
flag_find_huan_rightup_point
flag_find_huan_leftup_point :环岛的拐点是否找到的标志
ROUNDISLAND:这个是个常量，大家可以直接忽视
right_turn_down[0]
right_turn_down[1]:诸如此类的是拐点的数组，0存的是行数，1存的是列数
R_black[70]/L_black[70]:左右边线数组
calkuan[i]:第i行的宽度
truepianfangflag [i]:方差是否满足标志位，大家可以看环岛辅助判断的文章，有具体方法
leftk :左边线之前算出的斜率
break_hangshu:左边线的有效行数
huandao_flag ：是否判断到环岛的标志
huandao_flag_R_L:左环还是右环
crossrecord
crossderecord
cross31 :这三个没有用 大家直接忽略
parameterB :斜率
*/
void juge_left_succesive_and_if_k_limit() //右环岛
{ shifouoyouzhongdian=flag_find_huan_rightmiddle_point;
youwushuangduandian= truedoubleright;
if (shifouyoubanmaxian == 0 && rightduan != 0&&huandaotype!=0 && turepodaoflag == 0 && rukuflag == 0 && firstku==0&&countfirstku==0)//不处于其他元素状态
{
if (huandao_memory!=0 || (rightentirewhite==1&&doublewhite==0&&truedoubleright == 1&&continueleft!=0&&continueright==0))//出现右边双断点或者进入了环岛状态
{
if (huandao_memoryforleft == 0)//没有进入左环岛状态
{
int i;
float leftk=0;
cakkuandu();
flag_find_huan_leftdown_point = 0;
flag_find_huan_rightdown_point = 0;
flag_find_huan_rightmiddle_point = 0;
flag_find_huan_leftmiddle_point = 0;
flag_find_huan_rightup_point = 0;
flag_find_huan_leftup_point = 0;
if (huandao_memory ==1|| huandao_memory == 2||huandao_memory==0) //环岛找拐点开始 右下
{
if (huandaotype != 1&& huandaotype != 6)
{ findhuan_rightdown_point(10, horizon, ROUNDISLAND); }
if (huandaotype == 1)
{ findhuan_rightdown_point(10, horizon, ROUNDISLAND); }
if (huandaotype == 6)
{ findhuan_rightdown_point(3, horizon, ROUNDISLAND); }
}
if (huandao_memory == 3)
{
if (huandaotype == 1)
{ findhuan_rightdown_point(0, 32, ROUNDISLAND); }
if (huandaotype == 2)
{ findhuan_rightdown_point(0, 32, ROUNDISLAND); }
if (huandaotype == 3)
{ findhuan_rightdown_point(0, 32, ROUNDISLAND); }
if (huandaotype == 6)
{ findhuan_rightdown_point(0, 32, ROUNDISLAND); }
}
// if (huandao_memory == 4)
// {
// findhuan_rightdown_point(3, 20, ROUNDISLAND);
// }////////////////////////////////////////////////////////////以上根据环岛的类型和处于环岛的状态寻找右下的拐点
// findhuan_leftdown_point(5, 32, ROUNDISLAND); //左下
if (flag_find_huan_rightdown_point == 1 &&huandaotype==3)//找右中 右下已经下找到
{
if (huandao_memory == 2||huandao_memory == 3||huandao_memory == 0||huandao_memory == 1)
{ findhuan_rightmiddle_point(right_turn_down[0] + 5, horizon); }
if (huandao_memory == 4)
{ findhuan_rightmiddle_point2(2, horizon-15); }
}
if (flag_find_huan_rightdown_point == 0 && huandaotype == 3)//找右中，右下没找到
{
if (huandao_memory == 2||huandao_memory == 3||huandao_memory == 0||huandao_memory == 1)
{ findhuan_rightmiddle_point(3, horizon); }
if (huandao_memory == 4)
{
findhuan_rightmiddle_point2(2, horizon-15);
}
}
//
// if (flag_find_huan_leftdown_point == 1 && (left_turn_down[0] + 3) < 45) //找左中 左下找到
// { findhuan_leftmiddle_point(left_turn_down[0] + 3, horizon-8); }
// if (flag_find_huan_leftdown_point == 0)//找左中，左下没找到
// { findhuan_leftmiddle_point(3, horizon-8); }
if (flag_find_huan_rightmiddle_point == 1 )//右中找到，找右上
{
findhuan_rightup_point(10, horizon, ROUNDISLAND);
}
if (flag_find_huan_rightmiddle_point == 0)//右中没找到，找右上
{
findhuan_rightup_point(10, horizon, ROUNDISLAND);
}
// if (flag_find_huan_leftmiddle_point == 1 && (left_turn_middle[0] + 2) < horizon) //找左上，左中找到
// { findhuan_leftup_point(left_turn_middle[0] + 2, horizon+5, ROUNDISLAND); }
// if (flag_find_huan_leftmiddle_point == 0)//左中没找到找左上
// { findhuan_leftup_point(25, horizon+5, ROUNDISLAND); }
if (huandao_memory == 4) { flag_find_huan_rightup_point = 0; findhuan_rightmiddle_point2(0, horizon-20); findhuan_rightup_point(10, horizon, ROUNDISLAND);}//环岛找拐点结束
if (huandao_memory == 6) { flag_find_huan_rightup_point = 0;findhuan_leftout_point(5, horizon, ROUNDISLAND); }//处于状态七，找出去的左下的点
if (huandao_memory == 7) { findhuan_leftout_point(5, horizon, ROUNDISLAND); }
if (huandao_memory == 7) { findhuan_rightup_point(3, horizon, ROUNDISLAND); }//找右上
if (huandao_memory <= 8)
{
/*******行驶到环岛出口或者在行驶到环岛出口前********/
if (huandao_memory <= 3)
{ // 30
if (huandao_memory == 0 && right_turn_down[0] > 37 &&
flag_find_huan_rightdown_point == 1 && continueleft == 1
&& huandao_memoryforleft == 0
&&break_hangshu>=37)
{
huandao_flag = 1;
huandao_flag_R_L = 2;
huandao_memory = 1;
// SetText("行驶到环岛出口前（没进环岛呢）(远)");判断是否前方为环岛，如是，进入状态1
}
// else if (huandao_memory == 1 && ( right_turn_down[0] > 33 && flag_find_huan_rightdown_point == 1) ||
// (right_turn_middle[0] <= horizon+7 && right_turn_middle[0] >horizon-5 && flag_find_huan_rightmiddle_point == 1 ) && break_hangshu >= 35)
// {
// huandao_flag = 1;
// huandao_flag_R_L = 2;
//// huandao_memory = 2;
// // SetText("行驶到环岛出口前（没进环岛呢）(远)555");
// }
//快速进入右边环岛3
else if ((huandao_memory == 0 || huandao_memory == 1) && ((right_turn_middle[0] > 35 && right_turn_middle[0] <= horizon+5&&huandaotype!=6&&huandaotype!=1)) &&
flag_find_huan_rightmiddle_point == 1 && break_hangshu >= 37 && ( continueleft == 1 && huandao_memoryforleft == 0 )
&&right_turn_down[0] >= 18 )/*&& *///判断条件可能要改
{
huandao_flag = 1;
huandao_flag_R_L = 2;
huandao_memory = 3;//看看是否进入状态3
// SetText("break_hangshu "+ break_hangshu);
// SetText("right_turn_middle[0] "+ right_turn_middle[0]);
// SetText("快速进入右边环岛3");
}
else if (huandao_memory == 3 && (flag_find_huan_rightmiddle_point2 == 1&&right_turn_middle2[0] >= 0 && right_turn_middle2[0] <= 40) &&huandaotype==3)
{
huandao_flag = 1;
huandao_flag_R_L = 2;
flag_rukou = 1;
huandao_memory = 4;
// SetText("行驶到环岛出口前（没进环岛呢）(远)22");
}
else if (huandao_memory == 3 &&flag_find_huan_rightup_point==1&&huandaotype==3)
{
huandao_flag = 1;
huandao_flag_R_L = 2;
flag_rukou = 1;
huandao_memory = 4;
// SetText("行驶到环岛出口前（没进环岛呢）(远)22");
}
}
else
{
if (huandao_memory == 4 &&right_turn_up[0]<=48 && huandaotype == 3)
{
huandao_flag = 1;
flag_rukou2 = 1;
huandao_memory = 5;
// SetText("行驶到环岛入口（没进环岛呢）");
}
else if (huandao_memory == 5 && break_hangshu <= 45&&huandaotype!=6)///////可能后续要改
{
huandao_flag = 1;
huandao_memory = 6;
flag_rukou = 3;
}
else if (huandao_memory == 6 && left_turn_out[0] <= 40 && flag_find_huan_leftout_point == 1 )
{
huandao_flag = 1;
huandao_memory = 7;
//SetText("出环了");
}
else if (huandao_memory == 7 && flag_find_huan_rightup_point == 1 && flag_find_huan_leftout_point==0&& huandaotype!=6)
{
huandao_flag = 1;
huandao_memory = 8;
}
else if (huandao_memory == 8&&( continueright == 1||right_turn_up[0]<=10 ))
{
huandao_memory = 9;
huandao_memory = 0;
huandao_flag = 0;
huandao_procedure_variable = 0;///////////////////////////////以上都为判断环岛状态条件
}
}
}
huandaozhuangtai=huandao_memory;
R_mend_leftline_right_island((int)huandao_memory, (int)huandao_flag_R_L);//补线
R_mend_rightline_right_island((int)huandao_memory, (int)huandao_flag_R_L);//补线
// if (huandao_memory == 5 || huandao_memory == 6)
// {
// advanced_regression(1, 6, 8, 10, 12);
//
// if (parameterB == 0)
// {
// advanced_regression(1, 0, 2, right_turn_up[0], right_turn_up[0] + 1);
// leftlinefuzhi(parameterA, parameterB, 0, 60);
// }
//
// }/////////////////////////////若为状态5或6，进行拟合线
last_memory = huandao_memory;
int j = 0;
if (huandao_memory <= 8 && huandao_memory != 0)
{
for (j = 0; j < 70; j++)
{
LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);/////////////画中线
}
}
}
}
}
}
//======================================右环岛主体结束==================================
//=======================================右环岛补左线开始==============================
void R_mend_leftline_right_island(int type, int flag_R_L)
{
if (flag_R_L == 2)
{
// SetText("TYPE:" + type);
if (type == 5) //重新扫线，从左边往右边扫 取消掉一半
{
int j;
int delta = right_turn_up[0] - 0;
if (delta == 0) delta = 1;
float k = (right_turn_up[1] - L_black[0]) * 1.0f / (delta * 1.0f);//////////////改
// setText用户自定义("斜率" + k);
float b = L_black[0] ;//可能要改
for (j = (int)0; j <= (int)right_turn_up[0]; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_black[j] = (unsigned char)jicun;
}
}
// else if (type == 6) //状态6正常处理
// { regression(1,left_turn_out[0]-1,left_turn_out[0]);
// leftlinefuzhi(parameterA, parameterB, left_turn_out[0], horizon-10);
// int j = 0;
// int youguai_y = 0;
// int youguai_x = 0;
// int leftguai_y = 0;
// int leftguai_x = 0;
// /***找左下拐点***********/
// for (j = 2; j <= 60; j++)
// {
// if (L_black[j - 1] - L_black[j - 2] <= 0 && L_black[j] - L_black[j - 1] >= 0 && (L_black[j + 1] - L_black[j] > 0)
// && leftflag[j - 2] == 1 && leftflag[j - 1] == 1 && leftflag[j] == 1)
// {
// leftguai_y = (int)(j - 1);
// leftguai_x = L_black[j - 1];
// break;
// }
// }
// for (j = 8; j <= 60; j++)
// {
// //右连接处
// if (R_black[j] != 0)
// {
// youguai_y = j;
// youguai_x = R_black[j];
// break;
// }
// }
//
// if (leftguai_y != 0 && leftguai_x != 0 && youguai_y != 4 && youguai_x != 0)
// {
// //SetText("找到右上拐点");
// //SetText("右上拐点坐标：" + youguai_y + " " + youguai_x);
// //SetText("找到左下拐点");
// //SetText("左下拐点坐标：" + leftguai_y + " " + leftguai_x);
// if (youguai_y >= leftguai_y + 4)
// {
// jiansu_flag = 1;
// }
// else jiansu_flag = 0;
// //开始拉线
// int delta = youguai_y - leftguai_y;
// if (delta == 0) delta = 1;
// float k = (youguai_x - leftguai_x) * 1.0f / delta;
// float b = leftguai_x - k * leftguai_y;
// if (youguai_y >= leftguai_y)
// {
// for (j = (int)leftguai_y; j <= (int)youguai_y; j++)
// {
// int jicun = ((int)(k * j + b));
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// L_black[j] = (unsigned char)jicun;
// }
// }
// }
}
else if (type == 7) //状态7拉线
{
// int j;
// int delta = left_turn_out[0] - 70;
// if (delta == 0) delta = 1;
// float k = (left_turn_out[1] -0) * 1.0f / (delta * 1.0f);//////////////改
// // setText用户自定义("斜率" + k);
// float b = left_turn_out[1]-k*left_turn_out[0] ;//可能要改
// for (j = (int)0; j <= (int)right_turn_up[0]; j++)
// {
// int jicun = ((int)(k * j + b));
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// L_black[j] = (unsigned char)jicun;
// }
advanced_regression(2,left_turn_out[0]-3,left_turn_out[0]-2,left_turn_out[0]-1,left_turn_out[0]);
leftlinefuzhi(parameterA,parameterB,0,left_turn_out[0]);
}
else if (type == 8)
{
flag_blank_out_huandao = 0;
}
}
//========================================右环岛左线结束===============================
//=========================================右环岛右线开始==================================
void R_mend_rightline_right_island(int type, int flag_R_L)
{
int i = 0;
flag_R_L = 2;
if (flag_R_L == 2)
{
if (type == 1 || type == 2)//状态机1和2操作从下拐点开始往上拉，拉到上拐点下五行 type右线赋值操作
{
int j = 0;
if (right_turn_down[0] >= 3 && right_turn_middle[0] >= 2 && right_turn_middle[0] >= right_turn_down[0])
{
advanced_regression(2, right_turn_down[0] - 2, right_turn_down[0], right_turn_middle[0] - 1, right_turn_middle[0]);
for (j = (int)right_turn_down[0]; j <= (int)right_turn_middle[0]; j++)
{
int jicun = (int)(parameterB * j + parameterA);
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
R_black[j] = (unsigned char)jicun;
}
}
else
{
advanced_regression(2, 6, 8, 10, 12);
for (j = 0; j <= right_turn_down[0] + 10; j++)
{
int jicun = (int)(parameterB * j + parameterA);
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
R_black[j] = (unsigned char)jicun;
}
}
}
else if (type == 3) //状态机三操作---仍旧拉底下的线
{
int j = 0;
int delta = right_turn_middle[0] - 0;
if (delta == 0) delta = 1;
float k = (right_turn_middle[1] - R_black[0]) * 1.0f / delta;
float b = R_black[0];
for (j = (int)0; j <= (int)right_turn_middle[0]; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
R_black[j] = (unsigned char)jicun;
}
}
else if (type == 8)
{
int j = 0;
int delta = right_turn_up[0] - 0;
if (delta == 0) delta = 1;
float k = (right_turn_up[1] - R_black[0]) * 1.0f / delta;
float b = R_black[0];
for (j = (int)0; j <= (int)right_turn_up[0]; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
R_black[j] = (unsigned char)jicun;
}
}
// int begin = 1;
// for (i = 0; i < 50; i++)
// {
// if (R_black[i] == 0 && R_black[i + 1] == 0 && R_black[i + 2] != 0)
// {
// begin = i + 2;
// break;
// }
// }
//
// if (begin != 1)
// {
// if (R_black[begin + 7] != 0)
// {
// if (begin <= 27)
// { advanced_regression(2, begin + 5, begin + 6, begin + 7, begin + 8);
//
// }
// if (begin >=28)
// { regression(2, begin+3 , begin + 5);
//
// }
// monirightfuzhi(parameterA, parameterB, 0, 60);
// if (moniright[2] < L_black[2] && moniright[6] < L_black[6] && moniright[10] < L_black[10] && moniright[14] < L_black[14])
// { R_blackfuzhi(parameterA, parameterB, 0, begin + 2); }
// }
// else
// {
// if (begin <= 27)
// { advanced_regression(2, begin + 5, begin + 6, begin + 7, begin + 8);
//
// }
// if (begin >= 28)
// { regression(2, begin + 3, begin + 5);
//
// }
// monirightfuzhi(parameterA, parameterB, 0, 60);
// if (moniright[2] < L_black[2] && moniright[6] < L_black[6] && moniright[10] < L_black[10] && moniright[14] < L_black[14])
// { R_blackfuzhi(parameterA, parameterB, 0, begin + 2); }
// }
// }
// else
// {
// advanced_regression(2, 27, 28, 29, 30);
// monirightfuzhi(parameterA, parameterB, 0, 60);
// if (moniright[2] < L_black[2] && moniright[6] < L_black[6] && moniright[10] < L_black[10] && moniright[14] < L_black[14])
// { R_blackfuzhi(parameterA, parameterB, 0, 60); }
// }
else if (type == 7) //状态7拉线
{
advanced_regression(2,right_turn_up[0]+1,right_turn_up[0]+2,right_turn_up[0]+3,right_turn_up[0]+4);
R_blackfuzhi(parameterA,parameterB,0,right_turn_up[0]);
// for (int j = 0; j <= (int)right_turn_up[0]; j++)
// {
// int jicun = (int)(parameterB * j + parameterA);
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// R_black[j] = (unsigned char)jicun;
// }/////////上边的不行可以用这个
}
}
}
//============================================右环岛右线结束==================================
//==========================================找环岛拐点开始========================================
void findhuan_leftout_point(int start_point, int end_point, int RoadName) //找换到左下拐点 为左环岛而修改//右环岛出去的点
{
int j;
if (RoadName == ROUNDISLAND)
{
//setText用户自定义("l_start"+ l_start);
for (j = start_point; j <= end_point; j++)
{
//找左边突变(下拐点)；连续且是存在的点（）
if (L_black[j] - L_black[j - 2] <= 0&& L_black[j] - L_black[j - 3] <= 0 && L_black[j] - L_black[j + 3] <= 0 && L_black[j] - L_black[j + 4] <= 0
&& L_black[j] - L_black[j + 4] <= 0 && L_black[j] - L_black[j + 5] <= 0 && L_black[j] - L_black[j + 6] <= 0&& L_black[j] - L_black[j - 3] <= 0
)
{
int record = 0;
int i;
if (L_black[j - 1] == L_black[j - 2] && L_black[j - 2] == L_black[j - 3]) j = j - 1;
record = j;
for (i = j; i >= 1; i--)
{
if (L_black[i] == L_black[i - 1]) record = i - 1;
if (L_black[i] != L_black[i - 1]) break;
}
left_turn_out[0] = (int)(record);//数组里面没有第0行
left_turn_out[1] = L_black[record];
flag_find_huan_leftout_point = 1;
zuochujiance= left_turn_out[0];
break;
}
}
}
}
int flag_find_huan_rightout_point;
int right_turn_out [2];
void findhuan_rightout_point(int start_point, int end_point, int RoadName) //找环岛右下拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//找右边突变(下拐点)
if (R_black[j - 1]-R_black[j - 2]>=0 &&R_black[j-1]-R_black[j - 3]>=0 && R_black[j - 1] - R_black[j] >= 0 && R_black[j - 1] - R_black[j+1] >= 0
&& (R_black[j - 1] - R_black[j-4] >= 1&&R_black[j - 1] - R_black[j + 2] >= 1)
&& rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1 && rightflag[j+1] == 1 && rightflag[j -3] == 1 && rightflag[j -4] == 1 && rightflag[j +2] == 1
&& R_black[j - 1]<130&& R_black[j - 1] >10)
{
int record = 0;
int i;
record = j;
for (i = j; i >= 1; i--)
{
if (R_black[i] == R_black[i - 1]) record = i - 1;
if (R_black[i] != R_black[i - 1]) break;
}
right_turn_out[0] = (int)(record);//数组里面没有第0行
right_turn_out[1] = R_black[record];
flag_find_huan_rightout_point = 1;
break;
}
}
//如果右下拐点坐标不为空 且 环岛状态不是4 就认定找到环岛的右下拐点
}
}
void findhuan_rightdown_point(int start_point, int end_point, int RoadName) //找环岛右下拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//找右边突变(下拐点)
if (R_black[j]-R_black[j - 1]>= 0 && R_black[j]- R_black[j - 2]>=0 &&R_black[j] - R_black[j+2] >=0&& R_black[j] - R_black[j + 3] >= 0
&&( R_black[j] - R_black[j - 1]<=5 || R_black[j] - R_black[j-2] <= 5)&&j<=break_hangshu-3
&& (R_black[j] - R_black[j - 2] >= 1 || R_black[j] - R_black[j - 3] >= 1 || (j>=4&&R_black[j] - R_black[j - 4] >= 1) || (j >= 5&&R_black[j] - R_black[j - 4] >= 1))
&& R_black[j] - R_black[j + 4] >= 1 && R_black[j] - R_black[j + 5] >= 1
&& rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1 && R_black[j+2]<=4&&R_black[j+3]<=4&&R_black[j+4]<=4)
{
if (R_black[j] < 175)
{
int record = 0;
//int i;
record = j;
//for (i = j; i >= 1; i--)
//{
// if (R_black[i] == R_black[i - 1]) record = i - 1;
// if (R_black[i] != R_black[i - 1]) break;
//}
right_turn_down[0] = (int)(record);//数组里面没有第0行
right_turn_down[1] = R_black[record];
flag_find_huan_rightdown_point = 1;
youxiajiance=right_turn_down[0];
// SetText("找到环岛右下拐点 找到右下拐点" + right_turn_down[0] + " " + right_turn_down[1]);
break;
}
}
}
//如果右下拐点坐标不为空 且 环岛状态不是4 就认定找到环岛的右下拐点
}
}
void findhuan100_rightdown_point(int start_point, int end_point, int RoadName) //找环岛右下拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//找右边突变(下拐点)
if (R_black[j]-R_black[j - 1]>= 0 && R_black[j]- R_black[j - 2]>=0 &&R_black[j] - R_black[j+2] >=0&& R_black[j] - R_black[j + 3] >= 0
&&( R_black[j] - R_black[j - 1]<=5 || R_black[j] - R_black[j-2] <= 5)&&j<=break_hangshu-3
&& (R_black[j] - R_black[j - 2] >= 1 || R_black[j] - R_black[j - 3] >= 1 || (j>=4&&R_black[j] - R_black[j - 4] >= 1) || (j >= 5&&R_black[j] - R_black[j - 4] >= 1))
&& R_black[j] - R_black[j + 4] >= 1 && R_black[j] - R_black[j + 5] >= 1 && R_black[j] - R_black[j + 6] >= 1
&& rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1 && (R_black[j+1]<=4||R_black[j+3]<=4))
{
if (R_black[j] < 175)
{
int record = 0;
record = j;
//for (i = j; i >= 1; i--)
//{
// if (R_black[i] == R_black[i - 1]) record = i - 1;
// if (R_black[i] != R_black[i - 1]) break;
//}
right_turn_down[0] = (int)(record);//数组里面没有第0行
right_turn_down[1] = R_black[record];
flag_find_huan_rightdown_point = 1;
youxiajiance=right_turn_down[0];
break;
}
}
}
//如果右下拐点坐标不为空 且 环岛状态不是4 就认定找到环岛的右下拐点
}
}
void findhuan_leftdown_point(int start_point, int end_point, int RoadName) //找换到左下拐点 为左环岛而修改
{
int j;
if (RoadName == ROUNDISLAND)
{
//setText用户自定义("l_start"+ l_start);
for (j = start_point; j <= end_point; j++)
{
//找左边突变(下拐点)；连续且是存在的点（）
if( L_black[j]-L_black[j - 1]<= 0 && L_black[j]- L_black[j - 2]<=0 &&L_black[j] - L_black[j+2] <=0&& L_black[j] - L_black[j + 3] <= 0
&&( L_black[j] - L_black[j - 1]<=5 || L_black[j] - L_black[j-2] <= 5)&&j<=break_hangshu-3
&& (L_black[j] - L_black[j - 2] <= -1 || L_black[j] - L_black[j - 3] <= -1 || (j>=4&&L_black[j] - L_black[j - 4] <= -1) || (j >= 5&&L_black[j] - L_black[j - 4] <= -1))
&& L_black[j] - L_black[j + 4] <= 1 && L_black[j] - L_black[j + 5] <= -1
&& leftflag[j - 2] == 1 && leftflag[j - 1] == 1 && leftflag[j] == 1 && L_black[j+2]>=184&&L_black[j+3]>=184&&L_black[j+4]>=184)
{
int record = 0;
int i;
if (L_black[j - 1] == L_black[j - 2] && L_black[j - 2] == L_black[j - 3]) j= j - 1;
record = j;
for (i = j; i >= 1; i--)
{
if (L_black[i] == L_black[i - 1]) record = i - 1;
if (L_black[i] != L_black[i - 1]) break;
}
left_turn_down[0] = (int)(record);//数组里面没有第0行
left_turn_down[1] = L_black[record];
flag_find_huan_leftdown_point = 1;
// SetText("找到环岛左下拐点 找到左下拐点" + left_turn_down[0] + " " + left_turn_down[1]);
break;
}
}
}
}
void findhuan_rightmiddle_point(int start_point, int end_point) //找环岛右中拐点 此函数待调整
{
int i;
for (i = start_point; i <= end_point; i++)
{
//找you拐点
if (
(i >= 8 && (R_black[i ] - R_black[i-1] >= 0) && (R_black[i ] - R_black[i-2] >= 0)&&
(R_black[i ] - R_black[i-3] >= 0) && (R_black[i] - R_black[i + 4] >= 4) && (R_black[i ] - R_black[i+3] >= 0) && (R_black[i] - R_black[i+4] >= 0) && ( (R_black[i] - R_black[i-5] >= 1) || (R_black[i] - R_black[i-6] >= 1) || (R_black[i ] - R_black[i-7] >= 1) || (R_black[i ] - R_black[i-8] >= 1))
&& rightflag[i - 2] == 1 && rightflag[i - 1] == 1 && rightflag[i] == 1 && rightflag[i + 1] == 1 &&rightflag[i + 2] == 1
))
{
right_turn_middle[0] = (int)(i);
right_turn_middle[1] = R_black[i];
flag_find_huan_rightmiddle_point = 1;
youzhongjiance=right_turn_middle[1];
break;
}
}
}
void findhuan_rightmiddle_point2(int start_point, int end_point) //找环岛右中拐点2给状态四用 此函数待调整
{
int i;
for (i = start_point; i <= end_point; i++)
{
//找you拐点
if (
(i >= 1 && (R_black[i ] - R_black[i-1] >= 0) && (R_black[i ] - R_black[i-2] >= 0)&&
(R_black[ i ] - R_black[i-3] >= 0) && (R_black[i] - R_black[i + 4] >= 1) && (R_black[i ] - R_black[i+3] >= 0) && (R_black[i] - R_black[i+4] >= 0) && ( (R_black[i] - R_black[i-5] >= 1) &&(R_black[i] - R_black[i-6] >= 1) &&( (R_black[i ] - R_black[i-7] >= 1) || (R_black[i ] - R_black[i-8] >= 1)))
&& R_black[i-2]>8 &&R_black[i-1]>8 && R_black[i]>8 && R_black[i+1]>8&&R_black[i+2]>8 && R_black[i + 3 ]>8&&R_black[i + 4]>8&&R_black[i + 5]>7
))
{
right_turn_middle2[0] = (int)(i);
right_turn_middle2[1] = R_black[i];
flag_find_huan_rightmiddle_point2 = 1;
youzhongjiance2=flag_find_huan_rightmiddle_point2;
break;
}
}
}
void findhuan_leftmiddle_point(int start_point, int end_point) //找环岛左中拐点
{
int i;
for (i = start_point; i <= end_point; i++)
{
//找左拐点
if (i >= 8 && (L_black[i ] - L_black[i-1] <= -1) && (L_black[i ] - L_black[i-2] <= -1)&&
(L_black[i ] - L_black[i-8] <= -5) && (L_black[i] - L_black[i + 4] <= -5) && (L_black[i ] - L_black[i+3] <= 0) && (L_black[i] - L_black[i+4] <= 0) && ( (L_black[i] - L_black[i-5] <= -1) || (L_black[i] - L_black[i-6] <= -1) || (L_black[i ] - L_black[i-7] <= -1) || (L_black[i ] - L_black[i-8] <= -1))
&&R_black[i+2]>right_turn_down[1]+1&&R_black[i+3]>right_turn_down[1]+1&&R_black[i+3]>right_turn_down[1]-1
&& leftflag[i - 2] == 1 && leftflag[i - 1] == 1 && leftflag[i] == 1 && leftflag[i + 2] == 1 && leftflag[i + 3] == 1
)
{
int record = 0;
int j;
record = i;
for (j= i; j >= 1; j--)
{
if (L_black[j] == L_black[j - 1]) record = j - 1;
if (L_black[j] != L_black[j - 1]) break;
}
if (L_black[record] <= 178)
{
left_turn_middle[0] = (int)(record);
left_turn_middle[1] = L_black[record];
flag_find_huan_leftmiddle_point = 1;
break;
}
}
}
}
void findhuan_rightup_point(int start_point, int end_point, int RoadName)// 找环岛右上拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//右上拐点
if ((R_black[j] - R_black[j - 2]) >= 0 &&(R_black[j] - R_black[j - 3]) >= 0 &&(R_black[j] - R_black[j - 4]) >= 5 && My_Abs(R_black[j], R_black[j - 1]) >= 0 && My_Abs(R_black[j + 1], R_black[j]) <= 4&& My_Abs(R_black[j + 2], R_black[j]) <= 7&& My_Abs(R_black[j + 3], R_black[j]) <= 8
&& R_black[j+1]>=20 && R_black[j]>=20&&R_black[j-4]<=7&&R_black[j-3]<=7&& R_black[j+2]>=20&&R_black[j+3]>=20&&R_black[j+4]>=20
)//
{
right_turn_up[0] = (int)(j);
right_turn_up[1] = R_black[j ];
flag_find_huan_rightup_point = 1;
youshangjiance=right_turn_up[0];
// SetText("找到环岛右上拐点 找到右上拐点" + right_turn_up[0] + " " + right_turn_up[1]);
break;
}
}
}
}
void findhuan_leftup_point(int start_point, int end_point, int RoadName) //找环岛左上拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//左上拐点
if (L_black[j]-L_black[j -1]<=0 && L_black[j] - L_black[j + 1] <= 0 && L_black[j] - L_black[j + 2] <= 0 && L_black[j] - L_black[j - 2] <= 0
&&( L_black[j] - L_black[j + 3] <= -1 && L_black[j] - L_black[j - 3] <= -1)
&& leftflag[j] == 1 && leftflag[j-1] == 1 && leftflag[j-2] == 1 && leftflag[j-3] == 1
&& leftflag[j + 1] == 1 && leftflag[j + 2] == 1 && leftflag[j + 3] == 1)
{
left_turn_up[0] = (int)(j );//数组里面没有第0行
left_turn_up[1] = L_black[j ];
flag_find_huan_leftup_point = 1;
// SetText("找到环岛左上拐点77 找到左上拐点77 " + left_turn_up[0] + " " + left_turn_up[1]);
break;
}
}
}
}
void findhuan_rightup_point77(int start_point, int end_point, int RoadName)// 找环岛右上拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//右上拐点
if ((R_black[j] - R_black[j - 2]) >= 4 &&(R_black[j] - R_black[j - 3]) >= 5 &&(R_black[j] - R_black[j - 4]) >= 5 && My_Abs(R_black[j], R_black[j - 1]) >= 4 && My_Abs(R_black[j + 1], R_black[j]) <= 4
&& rightflag[j + 1] == 1 && rightflag[j] == 1
&& (huandao_memory != 4 || (huandao_memory == 4 &&j<break_hangshu-2)))
{
right_turn_up[0] = (int)(j - 1);
right_turn_up[1] = R_black[j - 1];
flag_find_huan_rightup_point = 1;
// SetText("找到环岛右上拐点77 找到右上拐点77" + right_turn_up[0] + " " + right_turn_up[1]);
break;
}
}
}
}
void findhuan_rightup_point44(int start_point, int end_point, int RoadName)// 找环岛右上拐点
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
if (R_black[j] >= 20)
{ //如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//右上拐点
if ((R_black[j] - R_black[j - 2]) >= 4 &&(R_black[j] - R_black[j - 3]) >= 5 &&(R_black[j] - R_black[j - 4]) >= 5 && My_Abs(R_black[j], R_black[j - 1]) >= 4 && My_Abs(R_black[j + 3], R_black[j]) <= 4&& My_Abs(R_black[j + 4], R_black[j]) <= 4
&& rightflag[j + 1] == 1 && rightflag[j] == 1&& rightflag[j + 2] == 1&& rightflag[j + 3] == 1
&& (huandao_memory != 4 || (huandao_memory == 4 &&j<break_hangshu-2)))
{
right_turn_up[0] = (int)(j - 1);
right_turn_up[1] = R_black[j - 1];
flag_find_huan_rightup_point = 1;
// SetText("找到环岛右上拐点 找到右上拐点44" + right_turn_up[0] + " " + right_turn_up[1]);
break;
}
}
}
}
}
void findhuan_leftup_point77(int start_point, int end_point, int RoadName) //找环岛左上拐点，左环岛用
{
int j;
if (RoadName == ROUNDISLAND)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//左上拐点
if ((L_black[j - 2] - L_black[j ]) >= 6 && My_Abs(L_black[j], L_black[j - 2]) >= 3 && My_Abs(L_black[j + 1], L_black[j]) <= 3
&& leftflag[j] == 1 && leftflag[j+1] == 1 && leftflag[j + 2] == 1
&&((huandao_memoryforleft!=4|| (huandao_memoryforleft == 4&&j<=break_hangshu-2))))
{
left_turn_up[0] = (int)(j - 1);//数组里面没有第0行
left_turn_up[1] = L_black[j - 1];
flag_find_huan_leftup_point = 1;
break;
}
}
}
}
//==========================================找环岛拐点结束==========================================
//==========================================左环岛主体开始==============================================
void juge_left_succesive_and_if_k_limitforleft()
{
if (huandaotype!=0&&turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && rukuflag == 0 && (threemode == 0 || threemode == 6 || threemode == 12) && firstku == 0&&countfirstku==0)
{
if (truedoubleleft != 0 || huandao_memoryforleft != 0)
{
if (huandao_memory == 0)
{ //可能是环岛的地带 -0.17
leftdown15white = 1;
int i;
for (i = 0; i < 15; i++)
{
if (L_black[i] != 186)
{
leftdown15white = 0;
break;
}
}
cakkuandu();
flag_find_huan_leftdown_point = 0;
flag_find_huan_rightdown_point = 0;
flag_find_huan_rightmiddle_point = 0;
flag_find_huan_leftmiddle_point = 0;
flag_find_huan_rightup_point = 0;
flag_find_huan_leftup_point = 0;
findhuan_rightdown_point(5, 32, ROUNDISLAND); ///环岛找拐点开始
if (huandaotype == 6&& (huandao_memoryforleft==6|| huandao_memoryforleft==7))
{
flag_find_huan_rightdown_point = 0;
findhuan100_rightdown_point(5, horizon-15, ROUNDISLAND);
} ///环岛找拐点开始}
if (huandao_memoryforleft !=4&&huandaotype!=3&& huandao_memoryforleft != 3) //左下 非70环岛
{ findhuan_leftdown_point(5, horizon-15, ROUNDISLAND); }
if ((huandao_memoryforleft == 4|| huandao_memoryforleft==3) && huandaotype != 3)
{ findhuan_leftdown_point(5, horizon-15, ROUNDISLAND); } //左下
if (huandao_memoryforleft != 4 && huandaotype == 3&&huandao_memoryforleft != 3 ) //左下 三环到
{
findhuan_leftdown_point(5, horizon-20, ROUNDISLAND);
}
if ((huandao_memoryforleft == 4|| huandao_memoryforleft == 3) && huandaotype == 3)
{
findhuan_leftdown_point(5, horizon-20, ROUNDISLAND);
} //左下
if (flag_find_huan_rightdown_point == 1 && (right_turn_down[0] + 1) < 45)//右中 下找到
{ findhuan_rightmiddle_point(right_turn_down[0] + 3, horizon); }
if (flag_find_huan_rightdown_point == 0)
{ findhuan_rightmiddle_point(3, horizon); }
if (flag_find_huan_leftdown_point == 1 && (left_turn_down[0] + 6) < 45&&huandaotype!=3) //左中 下找到 //四的时候中拐点拉近
{
if (huandao_memoryforleft == 4 && left_turn_down[0] + 6 <= 30)
{ findhuan_leftmiddle_point(left_turn_down[0] + 6, horizon); }
if(huandao_memoryforleft!=4) findhuan_leftmiddle_point(left_turn_down[0] + 6, horizon);
}
if (flag_find_huan_leftdown_point == 0&&huandaotype!=3)
{
if (huandao_memoryforleft == 4)
{ findhuan_leftmiddle_point(3, horizon); }
if (huandao_memoryforleft != 4)
{ findhuan_leftmiddle_point(3, horizon); }
} //左中
if (flag_find_huan_leftdown_point == 1 && (left_turn_down[0] ++) < 45 && huandaotype == 3) //左中 下找到
{
if (huandao_memoryforleft != 3)
{
findhuan_leftmiddle_point(left_turn_down[0] + 6, horizon);
}
if (huandao_memoryforleft == 3)
{
findhuan_leftmiddle_point(left_turn_down[0] + 6, horizon);
}
}
if (flag_find_huan_leftdown_point == 0 && huandaotype == 3)
{
if (huandao_memoryforleft != 3&& huandao_memoryforleft != 4)
{
findhuan_leftmiddle_point( 3, horizon);
}
if (huandao_memoryforleft == 3 || huandao_memoryforleft == 4)
{
if (leftdown15white == 1)
{ findhuan_leftmiddle_point(3, horizon); }
if (leftdown15white == 0)
{ findhuan_leftmiddle_point(3, horizon); }
}
} //左中
if (flag_find_huan_rightmiddle_point == 1 && (right_turn_middle[0] + 2) < 50)
{ findhuan_rightup_point(right_turn_middle[0] + 2, horizon, ROUNDISLAND); }
if (flag_find_huan_rightmiddle_point == 0)
{ findhuan_rightup_point(25, horizon, ROUNDISLAND); }
if (flag_find_huan_leftmiddle_point == 1 && (left_turn_middle[0] + 2) < 50)
{ findhuan_leftup_point(left_turn_middle[0] + 2, horizon, ROUNDISLAND); }
if (flag_find_huan_leftmiddle_point == 0)
{ findhuan_leftup_point(25, horizon, ROUNDISLAND); } //环岛找拐点结束
if (huandao_memoryforleft >= 5) { flag_find_huan_leftmiddle_point = 0; findhuan_leftmiddle_point(3, horizon); }
if ((huandao_memoryforleft == 7|| huandao_memoryforleft == 6) ) { flag_find_huan_rightout_point = 0; findhuan_rightout_point(5, horizon, ROUNDISLAND); }
if (break_hangshu >= 5) lcenter_5 = LCenter[break_hangshu - 4];
if (break_hangshu <= 5) lcenter_5 = 0;
int begin = 0;
for (i = 3; i < 50; i++)
{
if (L_black[i] == 186 && L_black[i + 1] == 186 && L_black[i + 2] != 186)
{
begin = i + 2;
break;
}
}
if (huandao_memoryforleft == 7 && begin > 3&&huandaotype!=6) { flag_find_huan_leftup_point = 0; findhuan_leftup_point77(3, begin+5, ROUNDISLAND); }
regression(2, break_hangshu - 14, break_hangshu - 12);
if (huandao_memoryforleft <= 8)
{
/*******行驶到环岛出口或者在行驶到环岛出口前********/
if (huandao_memoryforleft <= 3)
{
if (huandao_memoryforleft == 1 && left_turn_down[0] <= 22 && left_turn_down[0] > 0 && break_hangshu >= 35 && flag_find_huan_leftdown_point == 1)
{
huandao_memoryforleft = 2;
huandao_flag_R_L = 1;
}
else if (huandao_memoryforleft == 2)
{
if ((L_black[5] >= 160)
)
{
// SetText("行驶到环岛出口（没进环岛呢）");
huandao_memoryforleft = 3;
huandao_flag_R_L = 1;
}
}
else if (huandao_memoryforleft == 0 && left_turn_down[0] > 5 && left_turn_down[0] <= 40 &&
flag_find_huan_leftdown_point == 1 &&
(calkuan[14] > 105 || calkuan[15] > 105 || calkuan[16] > 105 || calkuan[17] > 105 || calkuan[18] > 105//宽度肯能后续要改
|| calkuan[19] > 105 || calkuan[20] > 105 || calkuan[21] > 105 || calkuan[22] > 105)
&& truepianfangflagforleft == 1 && continueright == 1 && huandao_memory == 0
&& break_hangshu >= 37)
{
huandao_memoryforleft = 1;
huandao_flag_R_L = 1;
//SetText("行驶到环岛出口前（没进环岛呢）(远)");
}
//快一些进入
else if ((huandao_memoryforleft == 0 || huandao_memoryforleft == 1) &&( left_turn_middle[0] > 5 && left_turn_middle[0] <= 35 &&
flag_find_huan_leftmiddle_point == 1 )&&
((L_black[14] > 182 && R_black[14] > 20) || (L_black[15] > 182 && R_black[15] > 20) || (L_black[16] > 182 && R_black[16] > 20) ||
(L_black[17] > 182 && R_black[17] > 20) || (L_black[18] > 182 && R_black[18] > 20) || (L_black[19] > 182 && R_black[19] > 20) ||
(L_black[20] > 182 && R_black[20] > 20) || (L_black[21] > 182 && R_black[21] > 20)||(calkuan[12]>155&& calkuan[13] > 155 && calkuan[14] > 155 &&
calkuan[15] > 155 && calkuan[16] > 155 && calkuan[17] > 155 ) ) && (truepianfangflagforleft == 1 && continueright == 1 && huandao_memory == 0)
&& break_hangshu >= 37&& left_turn_middle[0]<=break_hangshu-5)
{
huandao_memoryforleft = 3;
huandao_flag_R_L = 1;
}
else if (huandao_memoryforleft == 3 && ((left_turn_middle[0] >= 0 && left_turn_middle[0] <= 22&& flag_find_huan_leftmiddle_point==1) || (flag_find_huan_leftup_point==1&&left_turn_up[0]<50)) && huandaotype!=3)
{
huandao_memoryforleft = 4;
huandao_flag_R_L = 1;
}
else if (huandao_memoryforleft == 3 && ((left_turn_middle[0] >= 0 && left_turn_middle[0] <= 45 && flag_find_huan_leftmiddle_point == 1) ||(flag_find_huan_leftup_point == 1 && left_turn_up[0]<=50) ) && huandaotype == 3)
{
huandao_memoryforleft = 4;
huandao_flag_R_L = 1;
}
}
else if (huandao_memoryforleft == 4 && break_hangshu <= 33 && R_black[10] > 23&&huandaotype!=6)
{
huandao_memoryforleft = 5;
huandao_flag_R_L = 1;
// SetText("这里进去的5");
// SetText("行驶到环岛入口（没进环岛呢）");
}
else if (huandao_memoryforleft == 4 && break_hangshu <= horizon-20 && R_black[10] > 23 && huandaotype == 6)
{
huandao_memoryforleft = 5;
huandao_flag_R_L = 1;
// SetText("这里进去的5");
// SetText("行驶到环岛入口（没进环岛呢）");
}
else if (huandao_memoryforleft == 5 && break_hangshu <= horizon-20 && R_black[10] > 20 && huandaotype != 6)
{
huandao_memoryforleft = 6;
huandao_flag_R_L = 1;
// SetText("这里进去的6");
}
else if (huandao_memoryforleft == 5 && break_hangshu <= horizon-20 && R_black[10] > 20 && huandaotype == 6)
{
huandao_memoryforleft = 6;
huandao_flag_R_L = 1;
// SetText("这里进去的6");
}
else if (huandao_memoryforleft == 6 && right_turn_down[0] >= 1 && right_turn_down[0] <= horizon-20 && flag_find_huan_rightdown_point == 1&&huandaotype!=6 )
{
huandao_memoryforleft = 7;
huandao_flag_R_L = 1;
//SetText("出环了");
// SetText("这里进去的");
}
else if (huandao_memoryforleft == 6 && right_turn_down[0] >= 1 && right_turn_down[0] <= horizon-20 && flag_find_huan_rightdown_point == 1 && huandaotype == 6)
{
huandao_memoryforleft = 7;
huandao_flag_R_L = 1;
//SetText("出环了");
// SetText("这里进去的");
}
else if (huandao_memoryforleft == 7 && flag_find_huan_leftup_point == 1 && left_turn_up[1] < 180 && R_black[12] >= 15 && R_black[left_turn_up[0]] < 120
&& LCenter[10] > 80 && R_black[left_turn_up[0] + 4] > R_black[left_turn_up[0] + 2] && flag_find_huan_rightout_point == 0 && flag_find_huan_rightdown_point == 0)/**/
{
huandao_memoryforleft = 8;
huandao_flag_R_L = 1;
}
else if (huandao_memoryforleft == 8 && leftflag[13] == 1 && L_black[8] < 185 && L_black[9] < 185 && L_black[10] < 185
&& leftflag[14] == 1 && leftflag[15] == 1 && leftflag[16] == 1
&& L_black[13] < 185 && L_black[14] < 185 && L_black[15] < 185
&& L_black[16] < 185 && L_black[17] < 185 && (continueleft == 1 || (kuleftdownflag == 1 && kuleftupflag == 1) || (kurightupflag == 1 && kurightdownflag == 1)))
{
advanced_regression(2, 2, 4, 6, 8);
// SetText("8右线parameter斜率 " + parameterB);
if (parameterB < 2)
{
huandao_memoryforleft = 0;
huandao_flag_R_L = 0;
}
}
}
//SetText("flag_R_L " + huandao_flag_R_L);
R_mend_leftline_right_islandforleft((int)huandao_memoryforleft, (int)huandao_flag_R_L); //int
R_mend_rightline_right_islandforleft((int)huandao_memoryforleft, (int)huandao_flag_R_L);
//开启校验
if ((huandao_memoryforleft == 5 || huandao_memoryforleft == 6)&&huandaotype!=3)
{
advanced_regression(2, 6, 8, 10, 12);
if (parameterB == 0)
{
parameterA = 10;// 10 15
parameterB = 5;
R_blackfuzhi(parameterA, parameterB, 0, 60);
for (i = 0; i <= 60; i++)
{ L_black[i] = 186; }
}
//if (LCenter[20] < 120 && R_black[10] < 10)
//{
// parameterA = 10;// 10 15
// parameterB = 5;
// R_blackfuzhi(parameterA, parameterB, 0, 60);
// for (i = 0; i <= 60; i++)
// { L_black[i] = 186; }
//}
//if (LCenter[12] < 110 && R_black[10] < 10)
//{
// parameterA = 10;// 10 15
// parameterB = 5;
// R_blackfuzhi(parameterA, parameterB, 0, 60);
// for (i = 0; i <= 60; i++)
// { L_black[i] = 186; }
//}
}
last_memoryforleft = huandao_memoryforleft;
int j = 0;
if (huandao_memoryforleft <= 8 && huandao_memoryforleft != 0)
{
for (j = 0; j < 70; j++)
{
LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
}
}
}
}
}
}
//===========================================左环岛主体结束=============================================
//===========================================左环岛左线开始==============================================
void R_mend_leftline_right_islandforleft(int type, int flag_R_L)
{
int i = 0;
if (flag_R_L == 1)
{
if (type == 1 || type == 2)//状态机1和2操作从下拐点开始往上拉，拉到上拐点下五行 type右线赋值操作
{
int j = 0;
if (left_turn_down[0] >= 5 && left_turn_middle[0] >= 7 && left_turn_middle[0] > left_turn_down[0]&& flag_find_huan_leftmiddle_point==1&& flag_find_huan_leftdown_point==1)
{
advanced_regression(1, left_turn_down[0] - 2, left_turn_down[0], left_turn_middle[0] - 1, left_turn_middle[0]);
for (j = (int)left_turn_down[0]; j <= (int)left_turn_middle[0]; j++)
{
int jicun = (int)(parameterB * j + parameterA);
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_black[j] = (unsigned char)jicun;
}
}
else
{
if (left_turn_down[0] >= 3)
{
advanced_regression(1, left_turn_down[0] - 3, left_turn_down[0]-2, left_turn_down[0] - 1, left_turn_down[0]);
for (j = 0; j <= (int)left_turn_middle[0]; j++)
{
int jicun = (int)(parameterB * j + parameterA);
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_black[j] = (unsigned char)jicun;
}
}
}
}
else if (type == 3) //状态机三操作---仍旧拉底下的线
{
int j = 0;
if (flag_find_huan_leftmiddle_point == 1)
{
int delta = left_turn_middle[0] - 0;
//if (delta == 0) delta = 1;
float k = (left_turn_middle[1] - L_black[0]) * 1.0f / delta;
float b = L_black[0];
for (j = (int)0; j <= (int)left_turn_middle[0]; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_black[j] = (unsigned char)jicun;
}
}
else if (flag_find_huan_leftmiddle_point == 0 && flag_find_huan_leftdown_point == 1)
{
int delta = left_turn_down[0] - 0;
//if (delta == 0) delta = 1;
float k = (left_turn_down[1] - L_black[0]) * 1.0f / delta;
float b = L_black[0];
for (j = (int)0; j <= (int)left_turn_down[0]; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_black[j] = (unsigned char)jicun;
}
}
else
{
if (left_turn_middle[0] >= 5)
advanced_regression(1, left_turn_middle[0] - 5, left_turn_middle[0] -4, left_turn_middle[0] - 2, left_turn_middle[0]);
for (j = 0; j <= (int)left_turn_middle[0]; j++)
{
int jicun = (int)(parameterB * j + parameterA);
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_black[j] = (unsigned char)jicun;
}
}
}
else if (type == 8)
{
int begin = 1;
if (huandaotype != 6)
{
for (i = 0; i < 50; i++)
{
if (L_black[i] == 186 && L_black[i] == 186 && L_black[i + 2] != 186 && L_black[i + 3] != 186)
{
begin = i + 2;
int j;
for (j = begin; j < 50; j++)
{
if (L_black[j] < 177)
{
begin = j;
break;
}
}
break;
}
}
}
if (huandaotype == 6)
{
for (i = 0; i < 50; i++)
{
if (Pixels[i][165] == 1 && Pixels[i+1][ 165] == 1 && Pixels[i+2][ 165] != 1 && Pixels[i+3][165] != 1)
{
begin = i + 2;
int j;
for (j = begin; j < 50; j++)
{
if (L_black[j] < 177)
{
begin = j;
break;
}
}
break;
}
}
}
if (begin != 1)
{
if (L_black[begin + 7] != 0)
{
advanced_regression(1, begin + 3, begin + 5, begin + 6, begin + 7);
monileftfuzhi(parameterA, parameterB, 0, 60);
if (monileft[2] > R_black[2] && monileft[6] > R_black[6] && monileft[10] > R_black[10] && monileft[14] > R_black[14])
{ leftlinefuzhi(parameterA, parameterB, 0, begin + 2); }
}
else
{
advanced_regression(1, begin + 1, begin + 2, begin + 4, begin + 5);
monileftfuzhi(parameterA, parameterB, 0, 60);
if (monileft[2] > R_black[2] && monileft[6] > R_black[6] && monileft[10] > R_black[10] && monileft[14] > R_black[14])
{ leftlinefuzhi(parameterA, parameterB, 0, begin + 2); }
}
}
else
{
advanced_regression(1, 28, 29, 30, 31);
monileftfuzhi(parameterA, parameterB, 0, 60);
if (monileft[2] > R_black[2] && monileft[6] > R_black[6] && monileft[10] > R_black[10] && monileft[14] > R_black[14])
{ leftlinefuzhi(parameterA, parameterB, 0, 40); }
}
}
else if (type == 4)
{
if (huandaotype != 3 || leftdown15white == 0)
{
if (left_turn_middle[0] >= 5 && flag_find_huan_leftmiddle_point == 1&& left_turn_middle[0]<30)
{
advanced_regression(1, left_turn_middle[0] - 5, left_turn_middle[0] - 3, left_turn_middle[0] - 2, left_turn_middle[0]);
leftlinefuzhi(parameterA, parameterB, 0, (int)(left_turn_middle[0] + 1));
}
if (left_turn_down[0] >= 5 && flag_find_huan_leftmiddle_point == 0 && flag_find_huan_leftdown_point == 1)
{
advanced_regression(1, left_turn_down[0] - 5, left_turn_down[0] - 3, left_turn_down[0] - 2, left_turn_down[0]);
leftlinefuzhi(parameterA, parameterB, 0, (int)(left_turn_down[0] + 1));
}
}
if ( leftdown15white == 1)
{
}
}
else if (type == 7) //状态7拉线
{
for (i = 0; i <= 60; i++)
{ L_black[i] = 186; }
}
}
}
//=============================================左环岛左线结束=============================================
//============================================左环岛右线开始===========================================
void R_mend_rightline_right_islandforleft(int type, int flag_R_L) //int
{
if (flag_R_L == 1)
{
// SetText("TYPE:" + type);
if (type == 4) //状态机4的拉线 封住环岛前方赛道
{
int j;
int delta = left_turn_up[0] - 0;
if (delta == 0) delta = 1;
float k = (left_turn_up[1] - R_black[0]) * 1.0f / (delta * 1.0f);
// setText用户自定义("斜率" + k);
float b = R_black[0];
for (j = (int)0; j <= (int)left_turn_up[0]; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
R_black[j] = (unsigned char)jicun;
}
}
else if (type == 5) //重新扫线，从左边往右边扫 取消掉一半
{
}
else if (type == 6) //状态6正常处理
{
int j = 0;
int youguai_y = 0;
int youguai_x = 0;
int leftguai_y = 0;
int leftguai_x = 0;
/***找左下拐点***********/
for (j = 2; j <= 60; j++)
{
if (R_black[j - 1] - R_black[j - 2] >= 0 && R_black[j] - R_black[j - 1] <= 0 && (R_black[j + 1] - R_black[j] < 0)
&& rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1)
{
leftguai_y = (int)(j - 1);
leftguai_x = R_black[j - 1];
break;
}
}
for (j = 8; j <= 60; j++)
{
//右连接处
if (L_black[j] != 0)
{
youguai_y = j;
youguai_x = L_black[j];
break;
}
}
if (leftguai_y != 0 && leftguai_x != 0 && youguai_y != 4 && youguai_x != 0)
{
if (youguai_y >= leftguai_y + 4)
{
jiansu_flag = 1;
}
else jiansu_flag = 0;
//开始拉线
int delta = (int)(youguai_y - leftguai_y);
if (delta == 0) delta = 1;
float k = (youguai_x - leftguai_x) * 1.0f / delta;
float b = leftguai_x - k * leftguai_y;
if (youguai_y >= leftguai_y)
{
for (j = (int)leftguai_y; j <= (int)youguai_y; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
R_black[j] = (unsigned char)jicun;
}
}
}
}
else if (type == 7) //状态7拉线
{
//advanced_regression(2, 5, 7, 9, 11);
//SetText("parameterA7" + parameterA);
//SetText("parameterB7" + parameterB);
//parameterA = 0;// 10 15
//parameterB = 6;
//R_blackfuzhi(parameterA, parameterB, 0, 60);
int begin=3;
int i;
for (i = 1; i < 50; i++)
{
if (Pixels[i-1][170] == 1&&Pixels[i][ 170] == 1 && Pixels[i+1][ 170] != 1 && Pixels[i +2][170] != 1)
{
begin = i;
break;
}
}
if (begin == 3) { begin = 30; }
if (begin<=20) { begin = 22; }
// SetText("begin 77" + begin);
if (flag_find_huan_rightout_point == 1&& right_turn_out[1]>30)
{
R_black[begin] = 186;
R_black[begin - 1] = 150;
advanced_regression(2, right_turn_out[0] - 2, right_turn_out[0] , begin - 1, begin);
R_blackfuzhi(parameterA, parameterB, 0, 60);
}
// (flag_find_huan_rightout_point == 0)
else
{
R_black[begin] = 186;
R_black[begin - 1] = 150;
advanced_regression(2, 0, 3, begin - 1, begin);
R_blackfuzhi(parameterA, parameterB, 0, 60);
}
}
else if (type == 8)
{
flag_blank_out_huandao = 0;
}
}
}
//===========================================左环岛右线结束===============================================
//============================================斜十字补线开始：4==================================================
void buzhongxian4()//限制斜率
{
firstmid = 0;
if (turepodaoflag == 0 && rukuflag == 0 && (huandao_memory == 0 && huandao_memoryforleft == 0)&& haveleftku==0) //|| threemode == 3 || threemode == 9&&(bigup==0||(bigup!=0&&calkuan[bigup+1]<110 && calkuan[bigup + 2] < 110 && calkuan[bigup + 3] < 110))
{
// SetText("进了斜入这里0");
int min;
int hang;
leftdownflag3 = 0;
rightdownflag3 = 0;
leftupflag = 0;
rightupflag = 0;
firstmid = 0;
find_leftup_point(6, horizon, CROSSROAD);
find_rightup_point(6, horizon, CROSSROAD);
find_rightdown_point3(6, horizon, CROSSROAD);
find_leftdown_point3(6, horizon, CROSSROAD);
if (right_turn_down[0] - 5 >= 0)
{
// SetText("L_black[right_turn_down[0] - 2]" + L_black[right_turn_down[0] - 2]);
// SetText("L_black[right_turn_down[0] - 3]" + L_black[right_turn_down[0] - 3]);
// SetText("L_black[right_turn_down[0] - 4]" + L_black[right_turn_down[0] - 4]);
// SetText("L_black[right_turn_down[0] - 5]" + L_black[right_turn_down[0] - 5]);
// SetText("L_black[right_turn_down[0] - 1]" + L_black[right_turn_down[0] - 1]);
}
// SetText("leftupflag" + leftupflag);
// SetText("rightupflag" + rightupflag);
// SetText("left_turn_up[1]" + left_turn_up[1]);
// SetText("right_turn_down[0]" + right_turn_down[0]);
if (right_turn_down[0] >= 5&&calkuan[right_turn_down[0]]<=120)///////宽带判断可能要改
{
// SetText("进了斜入这里0.1");
if (rightdownflag3 == 1 && L_black[right_turn_down[0] - 2] >= (int)165 && L_black[right_turn_down[0] - 3] >= (int)165
&& L_black[right_turn_down[0] - 4] >= (int)165 && L_black[right_turn_down[0] - 5] >= 165 && L_black[right_turn_down[0] - 1] >= 165 &&( (leftupflag == 0 || (leftupflag == 1 && left_turn_up[1] > 160))|| (rightupflag == 0 && leftupflag == 1 && leftupflag == 1 && left_turn_up[1] > 140))
)//如果发现右下拐点 &&(right_turn_down[0]<=6||(right_turn_down[0] >6))
//if (rightdownflag3 == 1 )//如果发现右下拐点
{
min = right_turn_down[0];
// SetText("进了斜入这里1");
if (min <= 15)
{
advanced_regression(2, 0, 1, 2, 3);
if (parameterB > 0)
{
R_blackfuzhi(parameterA, parameterB, 0, 60);
// for (int j = 0; j <= 60; j++)
// {
// int jicun = (int)(parameterB * j + parameterA);
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// R_black[j] = (unsigned char)jicun;
// }/////////上边的不行可以用这个
for (hang = right_turn_down[0]; hang < 60; hang++)
{
L_black[hang] = 186;
}
zhongxianfuzhi(0, 60);
firstmid = 1;
// SetText("单右下拐点补线2345 ");
}
}
else if (min >= 15)
{
advanced_regression(2, min - 13, min - 11, min - 9, min - 7);
if (parameterB > 0)
{
R_blackfuzhi(parameterA, parameterB, min - 10, 60);
//for (int j = min-10; j <=60 ; j++)
// {
// int jicun = (int)(parameterB * j + parameterA);
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// R_black[j] = (unsigned char)jicun;
// }/////////上边的不行可以用这个
for (hang = right_turn_down[0]; hang < 60; hang++)
{
L_black[hang] = 186;
}
zhongxianfuzhi(0, 60);
firstmid = 1;
// SetText("单右下拐点补线min ");
}
}
}
}
if (left_turn_down[0] >= 5 && calkuan[left_turn_down[0]] <= 120)////////////宽度判断可能要改
{
// SetText("进了斜入这里20");
// SetText("leftdownflag3"+ leftdownflag3);
// SetText("LCenter[left_turn_down[0]]" + LCenter[left_turn_down[0]]);
// SetText("LCenter[left_turn_down[0] - 5]" + LCenter[left_turn_down[0] - 5]);
if (firstmid == 0 && leftdownflag3 == 1 && R_black[left_turn_down[0] - 2] <= 25
&& R_black[left_turn_down[0] - 3] <= 25 && R_black[left_turn_down[0] - 4] <= 25 && R_black[left_turn_down[0] - 5] <= 25 &&( (rightupflag == 0 || (rightupflag == 1 && right_turn_up[1] < 45)||(rightupflag == 1&&leftupflag==0&& rightupflag == 1 && right_turn_up[1] < 45)))
&&(left_turn_down[0]<=6||(left_turn_down[0]>6))
)//如果发现左下拐点
{
min = left_turn_down[0];
// SetText("进了斜入这里2");
if (min < 15)
{
advanced_regression(1, 0, 1, 2, 3);
if (parameterB < 0)
{
leftlinefuzhi(parameterA, parameterB, 0, 60);
//for (int j = 0; j <=60; j++)
// {
// int jicun = (int)(parameterB * j + parameterA);
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// L_black[j] = (unsigned char)jicun;
// }/////////上边的不行可以用这个
for (hang = left_turn_down[0]; hang < 60; hang++)
{
R_black[hang] = 0;
}
zhongxianfuzhi(0, 60);
firstmid = 1;
// SetText("rightupflag " + rightupflag);
// SetText("right_turn_up[1] " + right_turn_up[1]);
// SetText("单左下拐点补线2345 ");
}
}
if (min >= 15)
{
advanced_regression(1, min - 13, min - 11, min - 9, min - 7);
if (parameterB < 0)
{
leftlinefuzhi(parameterA, parameterB, min - 10, 60);
//for (int j = min-10; j <= 60; j++)
// {
// int jicun = (int)(parameterB * j + parameterA);
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// L_black[j] = (unsigned char)jicun;
// }/////////上边的不行可以用这个
for (hang = left_turn_down[0]; hang < 60; hang++)
{
R_black[hang] = 0;
}
zhongxianfuzhi(0, 60);
firstmid = 1;
// SetText("单左下拐点补线min ");
}
}
}
}
if (left_turn_down[0] >= 5)
{
// SetText("R_black[left_turn_down[0] - 2] " + R_black[left_turn_down[0] - 2]);
// SetText("R_black[left_turn_down[0] - 3] " + R_black[left_turn_down[0] - 3]);
// SetText("R_black[left_turn_down[0] - 4] " + R_black[left_turn_down[0] - 4]);
// SetText("R_black[left_turn_down[0] - 5] " + R_black[left_turn_down[0] - 5]);
// SetText("rightupflag " + rightupflag);
// SetText("right_turn_up[1] " + right_turn_up[1]);
}
zhongxianfuzhi(0, 60);
}
}
//===============================================斜十字补线结束===================================================
//==========================================斜十字找拐点开始===========================================================
void find_leftup_point(int start_point, int end_point, int RoadName)
{
int j;
if (RoadName == CROSSROAD)
{
if (end_point >= 45) end_point =45;
// for (j = start_point; j <=end_point; j++)
for (j = end_point; j >= start_point; j--)
{
if (L_black[j] > 0)
{
if ((L_black[j] - L_black[j - 2]) <= 0 &&(L_black[j] - L_black[j - 3]) <= 0 &&(L_black[j] - L_black[j - 4]) <= -5 && My_Abs(L_black[j], L_black[j - 1]) >= 0 && My_Abs(L_black[j + 1], L_black[j]) <= 4&& My_Abs(L_black[j + 2], L_black[j]) <= 7&& My_Abs(L_black[j + 3], L_black[j]) <= 8
&& L_black[j+1]<=175 && L_black[j]<=175&&L_black[j-4]>=180&&L_black[j-3]>=180&& L_black[j+2]<=175&&L_black[j+3]<=175&&L_black[j+4]<=175
)
{
int record;
record = L_black[j - 1];
if ((j - 1)>15&&record <186)
{
left_turn_up[0] = (int)(j - 1);//数组里面没有第0行
left_turn_up[1] = L_black[j - 1];
if (left_turn_up[1] < 186)
{
leftupflag = 1;
break;
}
}
if((j-1)<=15)
{
left_turn_up[0] = (int)(j - 1);//数组里面没有第0行
left_turn_up[1] = L_black[j - 1];
if (left_turn_up[1] < 186)
{
leftupflag = 1;
break;
}
}
}
}
//第二类
if (L_black[j] > 0&&j>=35)
{
if (L_black[j - 2] - L_black[j - 1] >= 23&& My_Abs(L_black[j], L_black[j - 1]) <= 10 && My_Abs(L_black[j + 1], L_black[j]) <=10 && ( L_black[j - 2] > 150 || L_black[j - 3] > 150||L_black[j - 4] > 150 || L_black[j - 5] > 150 || L_black[j - 6] > 150|| L_black[j - 3] - L_black[j - 1] >= 30)
&& leftflag[j - 1] == 1 && leftflag[j] == 1 && leftflag[j + 1] == 1)
{
int record;
record = L_black[j - 1];
if ((j - 1) > 15 && record < 180)
{
left_turn_up[0] = (int)(j - 1);//数组里面没有第0行
left_turn_up[1] = L_black[j - 1];
if (left_turn_up[1] < 182)
{
leftupflag = 1;
break;
}
}
if ((j - 1) <= 15)
{
left_turn_up[0] = (int)(j - 1);//数组里面没有第0行
left_turn_up[1] = L_black[j - 1];
if (left_turn_up[1] < 182)
{
leftupflag = 1;
break;
}
}
}
}
}
}
}
void find_rightup_point(int start_point, int end_point, int RoadName)/////找右上
{
int j;
if (RoadName == CROSSROAD)
{
for (j = start_point; j <= end_point; j++)
{
//如果出现了连续的行数超过45行判定为长直道，此时不存在拐点
//if (flag_s >= 47) break;
//右上拐点
if ((R_black[j] - R_black[j - 2]) >= 0 &&(R_black[j] - R_black[j - 3]) >= 0 &&(R_black[j] - R_black[j - 4]) >= 5 && My_Abs(R_black[j], R_black[j - 1]) >= 0 && My_Abs(R_black[j + 1], R_black[j]) <= 4&& My_Abs(R_black[j + 2], R_black[j]) <= 7&& My_Abs(R_black[j + 3], R_black[j]) <= 8
&& R_black[j+1]>=15 && R_black[j]>=15&&R_black[j-4]<=7&&R_black[j-3]<=7&& R_black[j+2]>=15&&R_black[j+3]>=15&&R_black[j+4]>=15
)//
{
right_turn_up[0] = (int)(j);
right_turn_up[1] = R_black[j ];
rightupflag=1;
// SetText("找到环岛右上拐点 找到右上拐点" + right_turn_up[0] + " " + right_turn_up[1]);
break;
}
//第二类
if (R_black[j] < 161)
{
if ((R_black[j - 1] - R_black[j - 2]) >= 10 && My_Abs(R_black[j], R_black[j - 1]) <= 8 && My_Abs(R_black[j + 1], R_black[j]) <= 8&&(R_black[j - 2] < 40 || R_black[j - 3] < 40 || R_black[j - 4] < 40 || R_black[j - 5] < 40 || R_black[j - 6] < 40)
&& rightflag[j - 1] == 1 && rightflag[j] == 1 && rightflag[j + 1] == 1&&j>=20)
{
int record;
record = R_black[j - 1];
if ((j - 1) > 15 && record > 4)
{
right_turn_up[0] = (int)(j - 1);
right_turn_up[1] = R_black[j - 1];
rightupflag = 1;
break;
}
if ((j - 1) <= 15)
{
right_turn_up[0] = (int)(j - 1);
right_turn_up[1] = R_black[j - 1];
rightupflag = 1;
break;
}
}
}
}
}
}
void find_leftdown_point3(int start_point, int end_point, int RoadName)
{
int j;
if (RoadName == CROSSROAD)
{
//setText用户自定义("l_start"+ l_start);
for (j = start_point; j <= end_point; j++)
{
//找左边突变(下拐点)；连续且是存在的点（）
if( L_black[j]-L_black[j - 1]<= 0 && L_black[j]- L_black[j - 2]<=0 &&L_black[j] - L_black[j+2] <=0&& L_black[j] - L_black[j + 3] <= 0
&&( L_black[j] - L_black[j - 1]<=5 || L_black[j] - L_black[j-2] <= 5)&&j<=break_hangshu-3
&& (L_black[j] - L_black[j - 2] <= -1 || L_black[j] - L_black[j - 3] <= -1 || (j>=4&&L_black[j] - L_black[j - 4] <= -1) || (j >= 5&&L_black[j] - L_black[j - 4] <= -1))
&& L_black[j] - L_black[j + 4] <= 1 && L_black[j] - L_black[j + 5] <= -1
&& leftflag[j - 2] == 1 && leftflag[j - 1] == 1 && leftflag[j] == 1 && L_black[j+2]>=184&&L_black[j+3]>=184&&L_black[j+4]>=184)
{
int record = 0;
int i;
if (L_black[j - 1] == L_black[j - 2] && L_black[j - 2] == L_black[j - 3]) j= j - 1;
record = j;
for (i = j; i >= 1; i--)
{
if (L_black[i] == L_black[i - 1]) record = i - 1;
if (L_black[i] != L_black[i - 1]) break;
}
left_turn_down[0] = (int)(record);//数组里面没有第0行
left_turn_down[1] = L_black[record];
leftdownflag3=1;
// SetText("找到环岛左下拐点 找到左下拐点" + left_turn_down[0] + " " + left_turn_down[1]);
break;
}
}
}
}
void find_rightdown_point3(int start_point, int end_point, int RoadName)//写入十字找拐点
{
int j;
if (RoadName == CROSSROAD)
{
for (j = start_point; j <= end_point; j++)
{
//找右边突变(下拐点)
if (R_black[j]-R_black[j - 1]>= 0 && R_black[j]- R_black[j - 2]>=0 &&R_black[j] - R_black[j+2] >=0&& R_black[j] - R_black[j + 3] >= 0
&&( R_black[j] - R_black[j - 1]<=5 || R_black[j] - R_black[j-2] <= 5)&&j<=break_hangshu-3
&& (R_black[j] - R_black[j - 2] >= 1 || R_black[j] - R_black[j - 3] >= 1 || (j>=4&&R_black[j] - R_black[j - 4] >= 1) || (j >= 5&&R_black[j] - R_black[j - 4] >= 1))
&& R_black[j] - R_black[j + 4] >= 1 && R_black[j] - R_black[j + 5] >= 1
&& rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1 && R_black[j+2]<=4&&R_black[j+3]<=4&&R_black[j+4]<=4)
{
if (R_black[j] < 175)
{
int record = 0;
//int i;
record = j;
//for (i = j; i >= 1; i--)
//{
// if (R_black[i] == R_black[i - 1]) record = i - 1;
// if (R_black[i] != R_black[i - 1]) break;
//}
right_turn_down[0] = (int)(record);//数组里面没有第0行
right_turn_down[1] = R_black[record];
rightdownflag3=1;
// SetText("找到环岛右下拐点 找到右下拐点" + right_turn_down[0] + " " + right_turn_down[1]);
break;
}
}
}
//如果右下拐点坐标不为空 且 环岛状态不是4 就认定找到环岛的右下拐点
}
}
//=========================================斜十字找拐点结束============================================
//================================================十字补线=================================
/////////////////////////////////有问题
void buzhongxian2_2()
{
se_thrmid = 0;
if ( doublewhite==1&&rukuflag == 0 && (huandao_memory == 0 && huandao_memoryforleft == 0) && haveleftku == 0)//|| threemode == 3 || threemode == 9
{
if (firstmid == 0)
{
int buzhongxianflag = 0;
int min = 0;
int max = 0;
float leftk = 0;
float rightk = 0;
int fanhui = 0;
int ewai = 0;
int flaghang = 0;
int zuida = 0;
leftdownflag = 0;
leftupflag = 0;
rightdownflag = 0;
rightupflag = 0;
buzhongxianmin = 6;
buzhongxianmax = 65;
int shizimax = 0;
if (break_hangshu >= 22) buzhongxianmax = break_hangshu - 2;
if (buzhongxianmax >= 43) buzhongxianmax = 65;
find_leftup_point((int)(6), (int)(buzhongxianmax), CROSSROAD);
youwuzuoshangdian=leftupflag;
youwuyoushangdian=rightupflag;
youwuzuoxiadian=leftdownflag;
youwuyouxiadian=rightdownflag;
if (leftupflag == 1)
{
if (buzhongxianmin >= left_turn_up[0] && left_turn_up[0] >= 4)
{
buzhongxianmin = left_turn_up[0] - 2;
}
find_leftdown_point((int)(buzhongxianmin), (int)(left_turn_up[0] - 1), CROSSROAD);
}
else if (leftupflag == 0) find_leftdown_point((int)(buzhongxianmin), (int)(buzhongxianmax), CROSSROAD);
find_rightup_point((int)(6), (int)(buzhongxianmax), CROSSROAD);
if (rightupflag == 1)
{
if (buzhongxianmin >= left_turn_up[0] && left_turn_up[0] >= 4)
{
buzhongxianmin = left_turn_up[0] - 2;
}
find_rightdown_point((int)(buzhongxianmin), (int)(right_turn_up[0] - 1), CROSSROAD);
}
else if (rightupflag == 0) find_rightdown_point((int)(buzhongxianmin), (int)(buzhongxianmax), CROSSROAD);
// SetText("buzhongxianmin " + buzhongxianmin);
// SetText("buzhongxianmax " + buzhongxianmax);
int tiaobianleft = 0;
int tiaobianright = 0;
int j;
if (leftdownflag == 1 && leftupflag == 1 && left_turn_down[0] >= 0)
{
for (j = left_turn_up[0]; j > left_turn_down[0]; j--)
{
if (My_Abs(L_black[j], L_black[j - 1]) >= 8)
{
tiaobianleft = 1;
break;
}
}
}
if (rightdownflag == 1 && rightupflag == 1 && right_turn_down[0] >= 0)
{
for (j = right_turn_up[0]; j > right_turn_down[0]; j--)
{
if (My_Abs(R_black[j], R_black[j - 1]) >= 8)
{
tiaobianright = 1;
break;
}
}
}
if (leftdownflag == 1 && rightdownflag == 1 && leftupflag == 1 && rightupflag == 1
&& right_turn_down[0] < right_turn_up[0] && left_turn_down[0] < left_turn_up[0]
&& (tiaobianleft == 1 || tiaobianright == 1)
&& right_turn_up[1] < left_turn_up[1]
)//如果同时发现4个拐点
{
if (left_turn_down[0] <= right_turn_down[0])
min = left_turn_down[0];
else
min = right_turn_down[0];
if (left_turn_up[0] <= right_turn_up[0])
max = right_turn_up[0];
else
max = left_turn_up[0];
if (left_turn_down[0] >= 2)
{
//advanced_regression(1, left_turn_down[0] - 2, left_turn_down[0], left_turn_up[0], left_turn_up[0] + 2);
regression(1, left_turn_down[0], left_turn_up[0]);
leftk = parameterB;
}
else
{
// advanced_regression(1, 0, left_turn_down[0], left_turn_up[0], left_turn_up[0] + 2);
regression(1, left_turn_down[0], left_turn_up[0]);
leftk = parameterB;
}
if (right_turn_down[0] >= 2)
{
// advanced_regression(2, right_turn_down[0] - 2, right_turn_down[0], right_turn_up[0], right_turn_up[0] + 2);
regression(2,right_turn_down[0], right_turn_up[0]);
rightk = parameterB;
}
else
{
// advanced_regression(2, 0, right_turn_down[0], right_turn_up[0], right_turn_up[0] + 2);
regression(2, right_turn_down[0], right_turn_up[0]);
rightk = parameterB;
}
fanhui = panduanfuhao(leftk, rightk);
if (fanhui == 0)
{
if (left_turn_down[0] >= 2)
{
advanced_regression(1, left_turn_down[0] - 2, left_turn_down[0], left_turn_up[0], left_turn_up[0] + 2);
if (left_turn_down[0] > 12) leftlinefuzhi(parameterA, parameterB, (int)(left_turn_down[0] - 2), (int)(left_turn_up[0] + 2));
if (left_turn_down[0] <= 12) leftlinefuzhi(parameterA, parameterB, 0, (int)(left_turn_up[0] + 2));
}
else
{
advanced_regression(1, 0, left_turn_down[0], left_turn_up[0], left_turn_up[0] + 2);
leftlinefuzhi(parameterA, parameterB, 0, (int)(left_turn_up[0] + 2));
}
if (right_turn_down[0] >= 2)
{
advanced_regression(2, right_turn_down[0] - 2, right_turn_down[0], right_turn_up[0], right_turn_up[0] + 2);
if (right_turn_down[0] > 12) R_blackfuzhi(parameterA, parameterB, (int)(right_turn_down[0] - 2), (int)(right_turn_up[0] + 2));
if (right_turn_down[0] <= 12) R_blackfuzhi(parameterA, parameterB, 0, (int)(right_turn_up[0] + 2));
}
else
{
advanced_regression(2, 0, right_turn_down[0], right_turn_up[0], right_turn_up[0] + 2);
R_blackfuzhi(parameterA, parameterB, 0, (int)(right_turn_up[0] + 2));
}
zhongxianfuzhi((char)(min), (char)(max));
se_thrmid = 1;
leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0;
shizimax = max;
}
if (fanhui == 1) { leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0; };
}
if (se_thrmid == 0 && leftdownflag == 0 && rightdownflag == 0 && leftupflag == 1 && rightupflag == 1
&& right_turn_up[1] < left_turn_up[1]
&&My_Abs(right_turn_up[0], left_turn_up[0])<=15
)//按照两个上拐点补线
{
int minup = 0;
if (left_turn_up[0] <= right_turn_up[0])
{
max = right_turn_up[0];
minup = left_turn_up[0];
}
else
{
max = left_turn_up[0];
minup = right_turn_up[0];
}
find_leftup_point((int)(6), (int)(minup + 5), CROSSROAD);
find_rightup_point((int)(6), (int)(minup + 5), CROSSROAD);
advanced_regression(1, left_turn_up[0], left_turn_up[0] + 1, left_turn_up[0] + 3, left_turn_up[0] + 4);
leftk = parameterB;
advanced_regression(2, right_turn_up[0], right_turn_up[0] + 1, right_turn_up[0] + 3, right_turn_up[0] + 4);
rightk = parameterB;
fanhui = panduanfuhao(leftk, rightk);
if (fanhui == 0 )////////宽度可能要改
{
//SetText(" 按两个上拐点补线： ");
if (left_turn_up[0] >= 35)
{
if(break_hangshu- left_turn_up[0]>4)
advanced_regression(1, left_turn_up[0], left_turn_up[0] + 1, left_turn_up[0] + 3, left_turn_up[0] + 4);
if (break_hangshu - left_turn_up[0]<= 4)
regression(1, left_turn_up[0], left_turn_up[0] + 2);
}
if (left_turn_up[0] >= 20 )
{ advanced_regression(1, left_turn_up[0], left_turn_up[0] + 1, left_turn_up[0] + 3, left_turn_up[0] + 4); }
if (left_turn_up[0] < 20)
{ advanced_regression(1, left_turn_up[0] + 2, left_turn_up[0] + 3, left_turn_up[0] + 5, left_turn_up[0] + 6); }
// regression(1, left_turn_up[0], left_turn_up[0] + 1);
leftlinefuzhi(parameterA, parameterB, 0, (int)(left_turn_up[0] + 1));
//for (int j = 0; j <= (int)(left_turn_up[0] + 1); j++)
// {
// int jicun = (int)(parameterB * j + parameterA);
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// L_black[j] = (unsigned char)jicun;
// }/////////上边的不行可以用这个
if (right_turn_up[0] >= 35)
{
if(break_hangshu- right_turn_up[0]>4)
advanced_regression(2, right_turn_up[0], right_turn_up[0] + 1, right_turn_up[0] + 3, right_turn_up[0] + 4);
if (break_hangshu - right_turn_up[0]<= 4)
regression(2, right_turn_up[0], right_turn_up[0] + 2);
}
if (right_turn_up[0] >= 20)
{ advanced_regression(2, right_turn_up[0], right_turn_up[0] + 1, right_turn_up[0] + 3, right_turn_up[0] + 4); }
if (right_turn_up[0] < 20)
{ advanced_regression(2, right_turn_up[0] + 2, right_turn_up[0] + 3, right_turn_up[0] + 5, right_turn_up[0] + 6); }
// regression(2, right_turn_up[0], right_turn_up[0] + 1);
R_blackfuzhi(parameterA, parameterB, 0, (int)(right_turn_up[0] + 1));
//for (int j = 0; j <= (int)(right_turn_up[0] + 1); j++)
// {
// int jicun = (int)(parameterB * j + parameterA);
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// R_black[j] = (unsigned char)jicun;
// }/////////上边的不行可以用这个
zhongxianfuzhi(0, 69);
se_thrmid = 1;
leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0;
shizimax = max;
}
if (fanhui == 1) { leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0; };
}
if (se_thrmid == 0 && leftdownflag == 0 && rightdownflag == 1 && leftupflag == 1 && rightupflag == 1
&& right_turn_down[0] < right_turn_up[0]
&& (tiaobianright == 1)
&& right_turn_up[1] < left_turn_up[1]
)//如果发现两个上拐点加右下拐点
{
min = right_turn_down[0];
max = right_turn_up[0];
ewai = left_turn_up[0];
if (right_turn_up[0] < left_turn_up[0]) zuida = left_turn_up[0];
if (right_turn_up[0] > left_turn_up[0]) zuida = right_turn_up[0];
advanced_regression(1, 3, 5, ewai, ewai + 2);
leftk = parameterB;
if (min >= 4)
{ advanced_regression(2, min - 4, min - 2, max, max + 2); }
if (min < 4)
{ advanced_regression(2, 1, 2, max, max + 2); }
rightk = parameterB;
fanhui = panduanfuhao(leftk, rightk);
if (fanhui == 0)
{
advanced_regression(1, 3, 5, ewai, ewai + 2);
leftlinefuzhi(parameterA, parameterB, (char)0, (char)(ewai));
//for (int j = 0; j <= (char)(ewai); j++)
// {
// int jicun = (int)(parameterB * j + parameterA);
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// L_black[j] = (unsigned char)jicun;
// }/////////上边的不行可以用这个
if (min >= 2)
{ advanced_regression(2, min - 2, min, max, max + 2); }
if (min < 2)
{ advanced_regression(2, 0, 1, max, max + 2); }
if(min>= 12)
R_blackfuzhi(parameterA, parameterB, (char)(min), (char)(max));
//for (int j = (char)(min); j <= (char)(max); j++)
// {
// int jicun = (int)(parameterB * j + parameterA);
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// R_black[j] = (unsigned char)jicun;
// }/////////上边的不行可以用这个
if (min < 12)
R_blackfuzhi(parameterA, parameterB, (char)(0), (char)(max));
//for (int j = 0; j <= (char)(max); j++)
// {
// int jicun = (int)(parameterB * j + parameterA);
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// R_black[j] = (unsigned char)jicun;
// }/////////上边的不行可以用这个
if (R_black[0] <= 4 && R_black[1] <= 4 && R_black[2] <= 4 && R_black[3] <= 4 && R_black[4] <= 4)
R_blackfuzhi(parameterA, parameterB, (char)(0), (char)(max));
//for (int j = 0; j <= (char)(max); j++)
// {
// int jicun = (int)(parameterB * j + parameterA);
// if (jicun >= 185) jicun = 185;
// else if (jicun <= 0) jicun = 0;
// R_black[j] = (unsigned char)jicun;
// }/////////上边的不行可以用这个
zhongxianfuzhi(0, 69);
se_thrmid = 1;
leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0;
shizimax = max;
}
if (fanhui == 1) { leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0; };
}
if (se_thrmid == 0 && leftdownflag == 1 && rightdownflag == 0 && leftupflag == 1 && rightupflag == 1
&& left_turn_down[0] < left_turn_up[0]
&& (tiaobianleft == 1)
&& right_turn_up[1] < left_turn_up[1]&&My_Abs(right_turn_up[0], left_turn_up[0])<=20
)//如果发现两个上拐点加左下拐点
{
min = left_turn_down[0];
max = left_turn_up[0];
ewai = right_turn_up[0];
if (right_turn_up[0] < left_turn_up[0]) zuida = left_turn_up[0];
if (right_turn_up[0] > left_turn_up[0]) zuida = right_turn_up[0];
if (min >= 4)
{ advanced_regression(1, min - 4, min - 2, max, max + 2); }
if (min < 4)
{ advanced_regression(1, 1, 2, max, max + 2); }
leftk = parameterB;
advanced_regression(2, 3, 5, ewai, ewai + 2);
rightk = parameterB;
fanhui = panduanfuhao(leftk, rightk);
if (fanhui == 0)
{
if (min >= 2)
{ advanced_regression(1, min - 2, min, max, max + 2); }
if (min < 2)
{ advanced_regression(1, 0, 1, max, max + 2); }
if(min>= 12)
leftlinefuzhi(parameterA, parameterB, (char)(min), (char)(max));
if (min < 12)
leftlinefuzhi(parameterA, parameterB, (char)(0), (char)(max));
if (L_black[0] == 186 && L_black[1] == 186 && L_black[2] == 186 && L_black[3] == 186 && L_black[4] == 186)
leftlinefuzhi(parameterA, parameterB, (char)(0), (char)(max));
advanced_regression(2, 3, 5, ewai, ewai + 2);
R_blackfuzhi(parameterA, parameterB, (char)0, (char)(ewai));
zhongxianfuzhi(0, 69);
se_thrmid = 1;
leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0;
shizimax = max;
}
if (fanhui == 1) { leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0; };
}
if (se_thrmid == 0 && leftdownflag == 1 && rightdownflag == 1 && leftupflag == 1 && rightupflag == 0
&& left_turn_down[0] < left_turn_up[0]
&& (tiaobianleft == 1)
)//如果发现两个下拐点加左上 左上左下 右下
{
min = left_turn_down[0];
max = left_turn_up[0];
ewai = right_turn_down[0];
int zuixiao = 0;
if (right_turn_down[0] < left_turn_down[0]) zuixiao = right_turn_down[0];
if (right_turn_down[0] > left_turn_down[0]) zuixiao = left_turn_down[0];
advanced_regression(1, min - 4, min - 2, max, max + 2);
leftk = parameterB;
if (ewai > 20) { advanced_regression(2, 5, 9, 14, 19); }
if (ewai > 10 && ewai <= 20)
{ advanced_regression(2, 3, 5, 7, 9); }
if (ewai <= 10)
{ advanced_regression(2, 1, 2, 3, 4); }
rightk = parameterB;
fanhui = panduanfuhao(leftk, rightk);
if (fanhui == 0)
{
advanced_regression(1, min - 4, min - 2, max, max + 2);
float leftparameterB = parameterB;
if (FMy_Abs(leftparameterB, 0) <= 2.3)
{
if (min >= 12)
leftlinefuzhi(parameterA, parameterB, (char)(min), (char)(max));
if (min < 12) leftlinefuzhi(parameterA, parameterB, (char)(0), (char)(max));
if (ewai > 20) { advanced_regression(2, 5, 9, 14, 19); }
if (ewai > 10 && ewai <= 20)
{ advanced_regression(2, 3, 5, 7, 9); }
if (ewai <= 10)
{ advanced_regression(2, 1, 2, 3, 4); }
R_blackfuzhi(parameterA, parameterB, 0, (int)(45));
zhongxianfuzhi(0, 45);
se_thrmid = 1;
leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0;
shizimax = max;
}
}
if (fanhui == 1) { leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0; };
}
if (se_thrmid == 0 && leftdownflag == 1 && rightdownflag == 1 && leftupflag == 0 && rightupflag == 1
&& right_turn_down[0] < right_turn_up[0]
&& (tiaobianright == 1)
)//如果发现两个下拐点加右上拐点 右上右下 左下
{
min = right_turn_down[0];
max = right_turn_up[0];
ewai = left_turn_down[0];
int zuixiao = 0;
if (right_turn_down[0] < left_turn_down[0]) zuixiao = right_turn_down[0];
if (right_turn_down[0] > left_turn_down[0]) zuixiao = left_turn_down[0];
if (ewai > 20) { advanced_regression(1, 5, 9, 14, 19); }
if (ewai > 10 && ewai <= 20)
{ advanced_regression(1, 3, 5, 7, 9); }
if (ewai <= 10)
{ advanced_regression(1, 1, 2, 3, 4); }
leftk = parameterB;
advanced_regression(2, min - 4, min - 2, max, max + 2);
rightk = parameterB;
fanhui = panduanfuhao(leftk, rightk);
if (fanhui == 0)
{
advanced_regression(2, min - 4, min - 2, max, max + 2);
float rightparameterB = parameterB;
if (FMy_Abs(rightparameterB, 0) <= 2.3)
{
if (ewai > 20) { advanced_regression(1, 5, 9, 14, 19); }
if (ewai > 10 && ewai <= 20)
{ advanced_regression(1, 3, 5, 7, 9); }
if (ewai <= 10)
{ advanced_regression(1, 1, 2, 3, 4); }
leftlinefuzhi(parameterA, parameterB, (int)(0), (int)(45));
advanced_regression(2, min - 4, min - 2, max, max + 2);
if (min >= 12)
R_blackfuzhi(parameterA, parameterB, (char)min, (char)(max));
if (min < 12)
R_blackfuzhi(parameterA, parameterB, 0, (char)(max));
zhongxianfuzhi(0, 45);
se_thrmid = 1;
leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0;
shizimax = max;
}
}
if (fanhui == 1) { leftdownflag = 0; rightdownflag = 0; leftupflag = 0; rightupflag = 0; };
}
buzhongxianflag = 0;
leftdownflag = 0;
leftupflag = 0;
rightdownflag = 0;
rightupflag = 0;
flaghang = 0;
min = 0;
max = 0;
leftk = 0;
rightk = 0;
ewai = 0;
zuida = 0;
}
}
int i;
//双十字初步处理开始
//if (secondmid == 1)
//{
// SetText("进入双子子判断");
// int seconmidleft = 0;
// int secondmidright =0;
// int sebeginright=0;
// int sebeginleft=0;
// for (i = 0; i < 10; i++)
// {
// if (R_black[i] <=4) secondmidright = secondmidright + 1;
// if (L_black[i] >=182) seconmidleft = seconmidleft + 1;
// }
// if (secondmidright >= 6)
// {
// SetText("进入双子子判断1");
// for (i = 3; i < 40; i++)
// {
// if (R_black[i] == 0 && R_black[i + 1] != 0 && R_black[i + 2] != 0)
// {
// sebeginright = i + 1;
// SetText("sebeginright" + sebeginright);
// break;
// }
// }
// if (i + 1 < 35)
// {
// advanced_regression(2, i + 3, i + 5, i + 7, i + 9);
// R_blackfuzhi(parameterA, parameterB, 0, i + 3);
// zhongxianfuzhi(0, i + 5);
// SetText("双十字补右线");
// }
// }
// if (seconmidleft >= 6)
// {
// SetText("进入双子子判断2");
// for (i = 3; i < 40; i++)
// {
// if (L_black[i] == 186 && L_black[i + 1] != 186 && L_black[i + 2] != 186)
// {
// sebeginleft = i + 1;
// SetText("sebeginleft" + sebeginleft);
// break;
// }
// }
// if (i + 1 < 35)
// {
// advanced_regression(1, i + 3, i + 5, i + 7, i + 9);
// leftlinefuzhi(parameterA, parameterB, 0, i + 3);
// zhongxianfuzhi(0, i + 5);
// SetText("双十字补左线");
// }
// }
//}
//双十字初步处理结束
//================================================十字防断处理开始==============================================
if (firstmid == 1 || secondmid == 1|| se_thrmid==1)
{
countbannothing = 1; //三帧之内不能掉线
// SetText(" firstmid secondmid se_thrmid "+ firstmid+" "+ secondmid + " " + se_thrmid);
for (i = 0; i < 69; i++)
{
LCenterrecord[i] = LCenter[i];
leftrecord[i] = L_black[i];
rightrecord[i] = R_black[i];
// SetText("L_black[i" + L_black[i]);
// SetText("leftrecord[i]" + leftrecord[i]);
}
}
if (countbannothing != 0) countbannothing = countbannothing + 1;
if (countbannothing == 5) countbannothing = 0;
if (countbannothing != 0) bannothing = 1;
if (countbannothing == 0) bannothing = 0;
if (bannothing == 1)
{
if (R_black[11] < 5 && R_black[12] < 5 && R_black[13] < 5 && L_black[11] > 181 && L_black[12] > 181 && L_black[13] > 181) //掉线了
{
for (i = 0; i < 69; i++)
{
LCenter[i] = (unsigned char)LCenterrecord[i];
L_black[i] = (unsigned char)leftrecord[i];
R_black[i] = (unsigned char)rightrecord[i];
}
}
if (R_black[16] < 5 && R_black[17] < 5 && R_black[18] < 5 && L_black[16] > 181 && L_black[17] > 181 && L_black[18] > 181) //掉线了
{
for (i = 0; i < 69; i++)
{
LCenter[i] = (unsigned char)LCenterrecord[i];
L_black[i] = (unsigned char)leftrecord[i];
R_black[i] = (unsigned char)rightrecord[i];
}
}
if (R_black[1] < 1 && R_black[2] < 1 && R_black[3] < 1 && L_black[1] > 185 && L_black[2] > 185 && L_black[3] > 185) //掉线了
{
for (i = 0; i < 69; i++)
{
LCenter[i] = (unsigned char)LCenterrecord[i];
L_black[i] = (unsigned char)leftrecord[i];
R_black[i] = (unsigned char)rightrecord[i];
}
}
if (R_black[20] < 1 && R_black[21] < 1 && R_black[22] < 1 && L_black[20] > 185 && L_black[21] > 185 && L_black[22] > 185) //掉线了
{
for (i = 0; i < 69; i++)
{
LCenter[i] = (unsigned char)LCenterrecord[i];
L_black[i] = (unsigned char)leftrecord[i];
R_black[i] = (unsigned char)rightrecord[i];
}
}
}
if(continueleft==0&&continueright==0)
{
liangbiandiuxian=1;
}
else
{
liangbiandiuxian=0;
}
// 十字防断处理结束
}
//================================================新的扫中线结束=================================
//=================================十字找拐点============================================================
void find_leftdown_point(int start_point, int end_point, int RoadName)
{
int j;
if (RoadName == CROSSROAD)
{ //setText用户自定义("l_start"+ l_start);
for (j = start_point; j <= end_point; j++)
{
//找左边突变(下拐点)；连续且是存在的点（）
if( L_black[j]-L_black[j - 1]<= 0 && L_black[j]- L_black[j - 2]<=0 &&L_black[j] - L_black[j+2] <=0&& L_black[j] - L_black[j + 3] <= 0
&&( L_black[j] - L_black[j - 1]<=5 || L_black[j] - L_black[j-2] <= 5)&&j<=break_hangshu-3
&& (L_black[j] - L_black[j - 2] <= 0 || L_black[j] - L_black[j - 3] <= 0 || (L_black[j] - L_black[j - 4] <= 0) || (L_black[j] - L_black[j - 4] <= 0))
&& L_black[j] - L_black[j + 4] <= 1 && L_black[j] - L_black[j + 5] <= -1
&& L_black[j+3]>=184 && L_black[j+4]>=184 && leftflag[j] == 1 && L_black[j+2]>=184)
{
int record = 0;
int i;
if (L_black[j - 1] == L_black[j - 2] && L_black[j - 2] == L_black[j - 3]) j= j - 1;
record = j;
for (i = j; i >= 1; i--)
{
if (L_black[i] == L_black[i - 1]) record = i - 1;
if (L_black[i] != L_black[i - 1]) break;
}
left_turn_down[0] = (int)(record);//数组里面没有第0行
left_turn_down[1] = L_black[record];
leftdownflag=1;
// SetText("找到环岛左下拐点 找到左下拐点" + left_turn_down[0] + " " + left_turn_down[1]);
break;
}
}
}
}
void find_rightdown_point(int start_point, int end_point, int RoadName)//得到拐点的行与列 int
{
int j;
if (RoadName == CROSSROAD)
{
for (j = start_point; j <= end_point; j++)
{
//找右边突变(下拐点)
if (R_black[j]-R_black[j - 1]>= 0 && R_black[j]- R_black[j - 2]>=0 &&R_black[j] - R_black[j+4] >=0&& R_black[j] - R_black[j + 5] >= 0
&&( R_black[j] - R_black[j - 1]<=5 || R_black[j] - R_black[j-2] <= 5)&&j<=break_hangshu-3
&& (R_black[j] - R_black[j - 2] >= 1 || R_black[j] - R_black[j - 3] >= 1 || (j>=4&&R_black[j] - R_black[j - 4] >= 1) || (j >= 5&&R_black[j] - R_black[j - 4] >= 1))
&& R_black[j] - R_black[j + 6] >= 1 && R_black[j] - R_black[j + 7] >= 1
&& rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1 && R_black[j+3]<=4&&R_black[j+4]<=4&&R_black[j+5]<=4)
{
if (R_black[j] < 175)
{
int record = 0;
//int i;
record = j;
//for (i = j; i >= 1; i--)
//{
// if (R_black[i] == R_black[i - 1]) record = i - 1;
// if (R_black[i] != R_black[i - 1]) break;
//}
right_turn_up[0] = (int)(j);
right_turn_up[1] = R_black[j ];
rightdownflag=1;
// SetText("找到环岛右下拐点 找到右下拐点" + right_turn_down[0] + " " + right_turn_down[1]);
break;
}
}
}
//如果右下拐点坐标不为空 且 环岛状态不是4 就认定找到环岛的右下拐点
}
}
int panduanfuhao(float x,float y)
{
int a;
a=0;
if(x<0&&y<0) a=1;
if(x>=0&&y>=0) a=1;
return a;
}
//=================================十字找拐点============================================================
//=====================================求两数之差绝对值的浮点数========================================
float FMy_Abs(float a, float b)
{
if ((a - b) > 0)
return ((float)(a - b));
else return ((float)(b - a));
}
//=====================================求两数之差绝对值的浮点数========================================
//===============================判断是否全白=========================================
void entirewhite()
{
for(int i=0;i<=horizon;i++)
{
if(R_black[i]<=5&&R_black[i+1]<=5&&R_black[i+2]<=5&&R_black[i+3]<=5&&R_black[i+4]<=5&&R_black[i+5]<=5&&R_black[i+6]<=5&&
R_black[i+7]<=5&&R_black[i+8]<=5&&R_black[i+9]<=5&&R_black[i+10]<=5&&R_black[i+11]<=5&&R_black[i+12]<=5&&R_black[i+13]<=5&&R_black[i+14]<=5&&
R_black[i+15]<=5
)
{
rightentirewhite=1;
}
if(L_black[i]>=181&&L_black[i+1]>=181&&L_black[i+2]>=181&&L_black[i+3]>=181&&L_black[i+4]>=181&&L_black[i+5]>=181&&L_black[i+6]>=181&&L_black[i+7]>=181&&L_black[i+8]>=181&&
L_black[i+9]>=181&&L_black[i+10]>=181&&L_black[i+11]>=181&&L_black[i+12]>=181&&L_black[i+13]>=181&&L_black[i+14]>=181&&L_black[i+15]>=181
)
{
leftentirewhite=1;
}
}
if(leftentirewhite==1&&rightentirewhite==1)
{
doublewhite==1;
}
}
//// //===================================小s判断==========================================================
//// void mysmalls()
//// {
//// int i;
////
////
//// flag_centre_right_point = 0;
//// flag_centre_left_point = 0;
//// centre_left_point[0] = 0;
//// centre_left_point[1] = 0;
//// centre_right_point[0] = 0;
//// centre_right_point[1] = 0;
//// //开始找起始点//
////
////
//// breakwrong = 0;
//// for (i = 6; i <= 25; i++)
//// {
//// if (My_Abs(LCenter[i], LCenter[i - 1]) >= 8)
//// {
//// breakwrong = 1;
//// break;
//// }
//// }
//// for (i = 12; i <= endright; i++)
//// {
//// if (flag_middle_S == 1 && i > 9 && flag_centre_right_point == 0
//// && (LCenter[i - 5] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 6] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 7] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 8] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 8] - LCenter[i - 4]) <= 20 //判断j-4是否为中线拐点，拐点在左边的那个
//// && (LCenter[i - 3] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 2] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 2] - LCenter[i - 4]) <= 20
//// && (LCenter[i - 1] - LCenter[i - 4]) >= 1
//// && (LCenter[i] - LCenter[i - 4]) >= 1)
//// {
//// centre_right_point[0] = (int)(i - 3);
//// centre_right_point[1] = LCenter[i - 4];
//// flag_centre_right_point = 1;
//// }
//// else if (flag_middle_S == 0 && i > 9 && flag_centre_right_point == 0
//// && (LCenter[i - 5] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 6] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 7] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 8] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 8] - LCenter[i - 4]) <= 20 //判断j-4是否为中线拐点，拐点在左边的那个
//// && (LCenter[i - 3] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 2] - LCenter[i - 4]) >= 0
//// && (LCenter[i - 2] - LCenter[i - 4]) <= 20
//// && (LCenter[i - 1] - LCenter[i - 4]) >= 1
//// && (LCenter[i] - LCenter[i - 4]) >= 1
//// && LCenter[i - 4] >= 19 && LCenter[i - 4] <= 165)
//// {
//// centre_right_point[0] = (int)(i - 3);
//// centre_right_point[1] = LCenter[i - 4];
//// flag_centre_right_point = 1;
//// }
//// else if (flag_middle_S == 1 && i > 36 && flag_centre_right_point == 0
//// && (LCenter[i - 3] - LCenter[i - 2]) >= 0
//// && (LCenter[i - 4] - LCenter[i - 2]) >= 0
//// && (LCenter[i - 5] - LCenter[i - 2]) >= 0
//// && (LCenter[i - 6] - LCenter[i - 2]) >= 0
//// && (LCenter[i - 6] - LCenter[i - 2]) <= 30 //判断j-4是否为中线拐点，拐点在左边的那个
//// && (LCenter[i - 1] - LCenter[i - 2]) >= 0)
//// {
//// centre_right_point[0] = (int)(i - 2);
//// centre_right_point[1] = LCenter[i - 2];
//// flag_centre_right_point = 1;
//// }
//// else if (flag_middle_S == 0 && i > 36 && flag_centre_right_point == 0
//// && (LCenter[i - 3] - LCenter[i - 2]) >= 0
//// && (LCenter[i - 4] - LCenter[i - 2]) >= 0
//// && (LCenter[i - 5] - LCenter[i - 2]) >= 0
//// && (LCenter[i - 6] - LCenter[i - 2]) >= 0
//// && (LCenter[i - 6] - LCenter[i - 2]) <= 30 //判断j-4是否为中线拐点，拐点在左边的那个
//// && (LCenter[i - 1] - LCenter[i - 2]) >= 0
//// && LCenter[i - 2] >= 19 && LCenter[i - 2] <= 165)
//// {
//// centre_right_point[0] = (int)(i - 2);
//// centre_right_point[1] = LCenter[i - 2];
//// flag_centre_right_point = 1;
//// }
////
//// //中线左拐点
//// if (flag_middle_S == 1 && i > 9 && flag_centre_left_point == 0
//// && (LCenter[i - 5] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 6] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 7] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 8] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 8] - LCenter[i - 4]) >= -20 //判断j-4是否为中线拐点，拐点在左边的那个
//// && (LCenter[i - 3] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 2] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 2] - LCenter[i - 4]) >= -20
//// && (LCenter[i - 1] - LCenter[i - 4]) <= -1
//// && (LCenter[i] - LCenter[i - 4]) <= -1)
//// {
//// centre_left_point[0] = (int)(i - 3);
//// centre_left_point[1] = LCenter[i - 4];
//// flag_centre_left_point = 1;
//// }
//// else if (flag_middle_S == 0 && i > 9 && flag_centre_left_point == 0
//// && (LCenter[i - 5] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 6] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 7] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 8] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 8] - LCenter[i - 4]) >= -20 //判断j-4是否为中线拐点，拐点在左边的那个
//// && (LCenter[i - 3] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 2] - LCenter[i - 4]) <= 0
//// && (LCenter[i - 2] - LCenter[i - 4]) >= -20
//// && (LCenter[i - 1] - LCenter[i - 4]) <= -1
//// && (LCenter[i] - LCenter[i - 4]) <= -1
//// && LCenter[i - 4] >= 19 && LCenter[i - 4] <= 165)
//// {
//// centre_left_point[0] = (int)(i - 3);
//// centre_left_point[1] = LCenter[i - 4];
//// flag_centre_left_point = 1;
//// }
//// else if (flag_middle_S == 1 && i > 36 && flag_centre_left_point == 0
//// && (LCenter[i - 3] - LCenter[i - 2]) <= 0
//// && (LCenter[i - 4] - LCenter[i - 2]) <= 0
//// && (LCenter[i - 5] - LCenter[i - 2]) <= 0
//// && (LCenter[i - 6] - LCenter[i - 2]) <= 0
//// && (LCenter[i - 6] - LCenter[i - 2]) >= -30 //判断j-4是否为中线拐点，拐点在左边的那个
//// && (LCenter[i - 1] - LCenter[i - 2]) <= 0)
//// {
//// centre_left_point[0] = (int)(i - 2);
//// centre_left_point[1] = LCenter[i - 2];
//// flag_centre_left_point = 1;
//// }
//// else if (flag_middle_S == 0 && i > 36 && flag_centre_left_point == 0
//// && (LCenter[i - 3] - LCenter[i - 2]) <= 0
//// && (LCenter[i - 4] - LCenter[i - 2]) <= 0
//// && (LCenter[i - 5] - LCenter[i - 2]) <= 0
//// && (LCenter[i - 6] - LCenter[i - 2]) <= 0
//// && (LCenter[i - 6] - LCenter[i - 2]) >= -30 //判断j-4是否为中线拐点，拐点在左边的那个
//// && (LCenter[i - 1] - LCenter[i - 2]) <= 0
//// && LCenter[i - 2] >= 19 && LCenter[i - 2] <= 165)
//// {
//// centre_left_point[0] = (int)(i - 2);
//// centre_left_point[1] = LCenter[i - 2];
//// flag_centre_left_point = 1;
//// }
//// }
//// if (flag_centre_left_point == 1)
//// {
////
//// }
//// if (flag_centre_right_point == 1)
//// {
////
//// }
//// turesmallsflag = 0;
////
//// if (flag_centre_left_point == 1 && flag_centre_right_point == 1 && My_Abs(centre_right_point[0], centre_left_point[0]) >= 4 && My_Abs(centre_right_point[0], centre_left_point[0]) <= 28
//// && My_Abs(centre_right_point[0], centre_left_point[0]) <= 49 && (centre_right_point[1] + centre_left_point[1]) > 90
//// && (centre_right_point[1] + centre_left_point[1]) < 292
//// && rukuflag == 0 && turepodaoflag == 0 && huandao_memory == 0&& huandao_memoryforleft==0&& truelongflag==0&&trueshortflag==0&&
//// (threemode == 0 || threemode == 6 || threemode == 12 )
//// && pianfangleft>10&& pianfangright>10&& endright >= 33&& cumulants2>3&& breakwrong==0
//// &&calkuan[10]<186 && calkuan[12] < 186 && calkuan[14] < 186 && calkuan[16] < 186 && calkuan[18] < 186
//// )
//// {
//// turesmallsflag = 1;
//// }
//// int smallstype1 = 0;
//// int smallstype2 = 0;
//// if (turesmallsflag == 1 && break_hangshu > 43)
//// {
//// smallstype1 = 1;
//// }
//// if (turesmallsflag == 1 && break_hangshu <=43)
//// {
//// smallstype2 = 1;
//// }
//// if (threemode != 0 || threemode != 6 || threemode != 12)
//// {
//// turesmallsflag = 0;
//// }
//// }
////
//// //===================================小s判断==========================================================
////
////
//// //====================================中s判断===========================================================
//// void findmiddle()
//// {
//// if ( (threemode == 0 || threemode == 6 || threemode == 12 )&&turepodaoflag == 0 && (countpodao == 0 || countpodao > 3) && rukuflag == 0 && (huandao_memory == 0 && huandao_memoryforleft == 0)
//// &&calkuan[10]<186 && calkuan[12] < 186 && calkuan[14] < 186 && calkuan[16] < 186 && calkuan[18] < 186
//// )
//// {
//// trueflag_middle_S = 0;
//// if (firstmid == 0 && secondmid == 0 && countstoplongg == 0)
//// {
//// int i = 0;
//// swrong = 0;
//// int lrerror = 0;
//// int lrerrorwrong = 0;
//// //找后11行的拐点
//// flag_middle_S = 0;
//// findturn = 0;
//// overflag = 0;
//// if (flag_centre_left_point == 1 && flag_centre_right_point == 1 && break_hangshu >= 35)
//// {
//// if (centre_left_point[0] < centre_right_point[0]) { min = centre_left_point[0]; max = centre_right_point[0]; finddirection = 1; }
//// else if ((centre_left_point[0] >= centre_right_point[0])) { min = centre_right_point[0]; max = centre_left_point[0]; finddirection = 2; }
//// for (i = (int)min; i <= max; i++)
//// {
//// if (My_Abs(LCenter[i - 1], LCenter[i]) >= 40) { swrong = 1; }
//// }
//// //for (i = (int)max; i < 69; i++)
//// //{
//// // if (R_black[i] != 0 && R_black[i] <= 20) { findturn = 1; SetText("中SS右弯道停止判断右弯道停止判断右弯道停止判断"); break; }
//// // if (L_black[i] != 0 && L_black[i] > 165) { findturn = 1; SetText("中SS左弯道停止判断左弯道停止判断左弯道停止判断"); break; }
//// //}
//// lrerror = (centre_left_point[1] - centre_right_point[1]);
//// if (lrerror >= 35) { lrerrorwrong = 1; }
////
//// if (findturn == 0 && swrong == 0 && (My_Abs(centre_left_point[0], centre_right_point[0]) >= 5) && (My_Abs(centre_left_point[0], centre_right_point[0]) <= 60) && (centre_left_point[1] - centre_right_point[1]) >= 8 && (centre_left_point[1] - centre_right_point[1]) <= 160)
//// {
////
//// //setText用户自定义("符合小S的中线拐点特征");
//// flag_middle_S = 1;
//// }
//// findturn = 0;
//// for (i = 69; i >= max; i--)
//// {
////
//// if (R_black[i] == 0 && R_black[i - 1] != 0) { overflag = (unsigned char)i; if (i == 0) i = 1; break; }
////
//// }
//// if (finddirection == 2)
//// {
//// if (My_Abs((unsigned char)LCenter[i - 1], R_black[i - 1]) >= 38 && R_black[i - 1] != 0&&i<=40) { findturn = 1; }
//// if (My_Abs((unsigned char)LCenter[i - 2], R_black[i - 2]) >= 38 && R_black[i - 2] != 0 && i <= 40) { findturn = 1; }
//// if (My_Abs((unsigned char)LCenter[i - 3], R_black[i - 3]) >= 38 && R_black[i - 3] != 0 && i <= 40) { findturn = 1; }
////
////
//// }
//// if (finddirection == 1)
//// {
//// if (My_Abs((unsigned char)LCenter[i - 1], L_black[i - 1]) >= 38 && L_black[i - 1] != 0 && i <= 40) { findturn = 1; }
//// if (My_Abs((unsigned char)LCenter[i - 2], L_black[i - 2]) >= 38 && L_black[i - 1] != 0 && i <= 40) { findturn = 1; }
//// if (My_Abs((unsigned char)LCenter[i - 3], L_black[i - 3]) >= 38 && L_black[i - 1] != 0 && i <= 40) { findturn = 1; }
//// }
//// if (flag_middle_S == 1 && findturn == 0 && lrerrorwrong == 1)
//// {
//// trueflag_middle_S = 1;
//// turesmallsflag = 0;
////
//// }
////
//// }
//// }
//// int last3trueflag_middle_S =(signed short int) last2trueflag_middle_S;
//// last2trueflag_middle_S = last1trueflag_middle_S;
//// last1trueflag_middle_S = trueflag_middle_S;
//// if (last1trueflag_middle_S == 1 && turesmallsflag == 0) { trueflag_middle_S = 1; }
//// if (last2trueflag_middle_S == 1 && turesmallsflag == 0) { trueflag_middle_S = 1; }
//// if (last3trueflag_middle_S == 1 && turesmallsflag == 0) { trueflag_middle_S = 1; }
//// }
////
//// if (threemode != 0 || threemode != 6 || threemode != 12)
//// { trueflag_middle_S = 0;
//// last1trueflag_middle_S = 0;
//// last2trueflag_middle_S = 0;
//// last3trueflag_middle_S = 0;
//// }
////
//// }
//////======================================中s判断=========================================================
////
////
//=======================================三岔路口判断====================================================
// void threeroad()
// {
// int hang;
//
// int i;
//
//
//
// for (hang = 1; hang < 69; hang++) //去掉杂点
// {
//
// if (LCenter[hang] == 0 && LCenter[hang + 1] == 0)
// {
//
// end = hang - 1;
// break;
// }
// if (My_Abs(LCenter[hang], LCenter[hang + 1]) > 20 && hang > 5)
// {
//
// end = hang - 1;
// break;
// }
// }
// countleft10 = 0;
// countright10 = 0;
// threekuanduEQ = 0;
// secondleft = 0;
// firstleft = 0;
// //三叉先行找一下拐点
//
//
//
//
// int start=end;
// for (i = end; i < 60; i++)
// {
// if (Pixels[i][ 93] == 0)
// {
// start = i;
// break;
// }
// }
//
//
//
//
// int allwhite = 0;
// int startallright = 0;
// for (i = 45; i > 0; i--)
// {
// if (Pixels[i + 3][2] == 0 && Pixels[i + 2][ 2] == 0 && Pixels[i + 1][2] == 1 && Pixels[i][ 2] == 1)
// {
// startallright = i+1;
// break;
// }
// }
//
// for (i = startallright; i > 0; i--)
// {
// if (Pixels[i][ 0] == 0)
// {
// allwhite = 1;
// break;
// }
// }
//
//
// int allwhiteright = 0;
// int startallleft = 0;
// for (i = 30; i > 0; i--)
// {
// if ( Pixels[i + 2][ 185] == 0 && Pixels[i + 1][ 185] == 1 )
// {
// startallleft = i + 1;
// break;
// }
// }
//
// for (i = startallleft; i > 0; i--)
// {
// if (Pixels[i][ 185] == 0)
// {
// allwhiteright = 1;
// break;
// }
// }
//
///*
//敲黑板!下面是三叉路口的思路，有助于理解代码
//
//首先:明确十字与三叉的区别
//大家可以想一下十字和三叉十字路口。十字路口的图像是一条白色直道直通到视角尽头，而三叉的白色直道道整个图像的中央就分叉开来了。
//其次:三叉路口辅助判断条件含义:
//(1)入口的两个下拐点:三叉和十字的下拐点判断条件是有细微差别的，大家可以具体对比一下。
//(2)endmiddle:就是白色直道的最终行数:我有四十行图像，十字的endmiddle会有37、38，而三叉的endmiddle只有不超过34这样出现分叉。
//(3)endmiddle改进建议:
//这里的取法是找到左右下拐点取中值向上延伸，如果想要改进这个条件的准确度。建议是取左右拐点较小的行数，从0到该行用最小二乘法拟合往下延伸寻找endmiddle。
//(辅助条件判断方法在另一篇文章中)
//最后:
//判断思路:
//(1)非环岛状态
//(2)最有边线的第0行必须在视野范围内(1-185)。因为在十字转弯的时候，很多情况下左下或者右下是全白的，这一个条件可以有效的排除部分十字。但也意味着想要判断三叉，必须要车身是正的。
//(因为斜入十字和三叉特别像，大家估计也发现了)
//(3)找到左右下拐点，且左右下拐点行数差必须要在一定范围之内。(车正 基本上左右下拐点差个5行最多了)
//(4)endmiddle小于37行(我一共42行，大家自己调整这个值)
//(5)
//calkuan 宽度数组 在左右下拐点行数较高的那行向上1 2 3 行宽度要大于100.
//(因为三叉后面呈现分叉状态,宽度会变大不少，宽度太小就不认为是三叉。)
//
//注:状态1-6 为一个完整的入三叉到出三叉过程，因为我的车要跑两圈 有状态7-12
//
//*/
//
// //147 10 25811
//
// if (bigup != 0&& haveleftku == 0&&threemode ==0&&threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 110 && calkuan[bigup + 2] > 110
// && calkuan[bigup + 3] > 110 && calkuan[bigup] > 100
// && (endmidddle<=37&& endmidddle !=0)&& huandao_memory==0&&huandao_memoryforleft==0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 15&&secondmid==0&&firstmid==0&&se_thrmid==0
// )//
//
// threemode = 1;//进入状态1 找点拉线
//
// if ((haveleftku == 0 && threemode == 1 && R_black[0]==0&&
// L_black[0]==186)
// && huandao_memory == 0 && huandao_memoryforleft == 0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)//&&(threerightdownflag==0|| threeleftdownflag==0)
// ) threemode = 2;//进入状态2 右下角拉线
//
// if (haveleftku == 0 && threemode == 2 &&(R_black[0]!= 0|| L_black[0] != 186) && (threerightdownflag == 0 || threeleftdownflag == 0)
// && huandao_memory == 0 && huandao_memoryforleft == 0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
// ) threemode = 3; //进入状态3 正常跑
//
// if (bigup != 0 && haveleftku == 0 && threemode == 3 && threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 100 && calkuan[bigup + 2] > 100
// && calkuan[bigup + 3] > 100 && calkuan[bigup] > 100
// && (endmidddle <= 37 && endmidddle != 0)
// && huandao_memory == 0 && huandao_memoryforleft == 0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 17 && secondmid == 0 && firstmid == 0 && se_thrmid == 0
// ) threemode = 4; //进入状态四 同1 找点拉线
//
// if (haveleftku == 0 && threemode == 4 && R_black[0] == 0 && L_black[0] == 186
// && huandao_memory == 0 && huandao_memoryforleft == 0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
// )//&& (threerightdownflag == 0 || threeleftdownflag == 0)
// {
// threemode = 5; //进入状态5 同2 右下角拉线
// }
// if (haveleftku == 0 && threemode == 5 && (R_black[0] != 0 || L_black[0] != 186) && (threerightdownflag == 0 || threeleftdownflag == 0)
// && huandao_memory == 0 && huandao_memoryforleft == 0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
// )
// {
// threemode = 6; //进入6 正常跑状态
// }
// if (bigup != 0 && haveleftku == 0 && threemode == 6 && threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 110 && calkuan[bigup + 2] > 110
// && calkuan[bigup + 3] > 110 && calkuan[bigup] > 100
// && (endmidddle <= 37 && endmidddle != 0)
// && huandao_memory == 0 && huandao_memoryforleft == 0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 15 && secondmid == 0 && firstmid == 0 && se_thrmid == 0
// ) threemode = 7;//进入状态7 找左下拐点拉线
//
// if (haveleftku == 0 && threemode == 7&&R_black[0] == 0 &&
// L_black[0] == 186
// && huandao_memory == 0 && huandao_memoryforleft == 0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
// )// && (threerightdownflag == 0 || threeleftdownflag == 0)
// threemode =8;//进入状态8 直接拉线
//
// if (haveleftku == 0 && threemode == 8 &&( L_black[0]!=186||R_black[0]!=0) && (threerightdownflag == 0 || threeleftdownflag == 0)
// && huandao_memory == 0 && huandao_memoryforleft == 0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
// ) threemode = 9; //进入状态9 正常跑
// // threemode = 9;
// if (bigup != 0 && haveleftku == 0 && threemode == 9 && threerightdownflag == 1 && threeleftdownflag == 1 && calkuan[bigup + 1] > 100 && calkuan[bigup + 2] > 100
// && calkuan[bigup + 3] > 100 && calkuan[bigup] > 100
// && (endmidddle <= 37 && endmidddle != 0)
// && huandao_memory == 0 && huandao_memoryforleft == 0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1) && My_Abs(threerightdown[0], threeleftdown[0]) <= 17 && secondmid == 0 && firstmid == 0 && se_thrmid == 0
// ) threemode = 10;//进入状态10 同6 找左下挂点拉线
//
// if (haveleftku == 0 && threemode == 10 && R_black[0] == 0 &&
// L_black[0] == 186
// && huandao_memory == 0 && huandao_memoryforleft == 0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
// )//&& (threerightdownflag == 0 || threeleftdownflag == 0)
// {
// threemode = 11;//进入状态11 同7 左下角拉线
// }
// if (haveleftku == 0 && threemode == 11 && (L_black[0] != 186 || R_black[0] != 0) && (threerightdownflag == 0 || threeleftdownflag == 0)
// && huandao_memory == 0 && huandao_memoryforleft == 0
// && turepodaoflag == 0 && (countpodao == 0 || countpodao > 1)
// ) //进入状态12 继续跑
// {
// threemode = 12; //无状态什么都不做继续跑
// }
//
//
//
//
//
// threemodeop(threemode);
// // threemodeop(threemode);
// if (givetime != 1)
// {
// threeroadextrale = threeroadextra;
// threeroadextrari = threeroadextra;
// givetime = 1;
// }
// if (threeroadextrale != 0 && threemode == 6)
// {
// threemode = 0;
// threeroadextrale = threeroadextrale - 1;
//
// }
// if(threeroadextrari != 0&&threemode==12)
// {
// threemode = 6;
// threeroadextrari = threeroadextrari - 1;
//
// }
//
// }
//
//// //=======================================三岔路口判断====================================================
//
//
//
// //==========================================三叉路口操作开始=============================================
// void threemodeop(int modetype)
// {
// int i;
// int j;
// if ((modetype == 1|| modetype ==4)&& threerightdown[1]>=1&& LCenter[end]>=1) //
// {
// int endmidddle=0;
// for (i = 0; i < 60; i++)
// {
// if (Pixels[i][ 93] == 0)
// {
// endmidddle = i;
// break;
// }
// }
// R_black[threerightdown[0] - 1] = (unsigned char)(threerightdown[1] - 1);
// R_black[threerightdown[0]]=(unsigned char)(threerightdown[1]);
// if (endmidddle <= 1) endmidddle = 1;
// R_black[endmidddle - 1] = 93;
// R_black[endmidddle] = 94;
// advanced_regression(2, threerightdown[0] - 1, threerightdown[0], endmidddle - 1, endmidddle);
// R_blackfuzhi(parameterA, parameterB, threerightdown[0] - 1, 50);
// for (i = 0; i < 60; i++)
// {
// if (L_black[i] < 130) L_black[i] = 186;
// }
// for (j = 0; j < 70; j++)
// {
// LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
// }
// for (i = threerightdown[0]-1; i < 60; i++)
// {
// if (LCenter[i]<93) LCenter[i] = 93;
// }
//
// }
// if (modetype == 2||modetype==5) //
// {
// int endmidddle = 0;
// for (i = 0; i < 60; i++)
// {
// if (Pixels[i][93] == 0)
// {
// endmidddle = i;
// break;
// }
// }
// if (endmidddle <= 1) endmidddle = 1;
// R_black[endmidddle - 1] = 90;
// R_black[endmidddle] = 93;
// advanced_regression(2, 0, 1, endmidddle - 1, endmidddle);
// R_blackfuzhi(parameterA, parameterB, 0, 50);
// for (i = 0; i < 60; i++)
// {
// if (L_black[i] < 130) L_black[i] = 186;
// }
// for (j = 0; j < 70; j++)
// {
// LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
// }
// for (i = 0; i < 60; i++)
// {
// if (LCenter[i] < 93) LCenter[i] = 93;
// }
// }
// if ((modetype == 7|| modetype == 10) && threeleftdown[1] >= 1 && LCenter[end] >= 1) //
// {
// int endmidddle = 0;
// for (i = 0; i < 60; i++)
// {
// if (Pixels[i][ 93] == 0)
// {
// endmidddle = i;
// break;
// }
// }
// if (endmidddle <= 1) endmidddle = 1;
// L_black[threeleftdown[0] - 1] = (unsigned char)(threeleftdown[1] - 1);
// L_black[threeleftdown[0]] = (unsigned char)(threeleftdown[1]);
// if (endmidddle <= 1) endmidddle = 1;
// L_black[endmidddle - 1] = 93;
// L_black[endmidddle] = 91;
// advanced_regression(1, threeleftdown[0] - 1, threeleftdown[0], endmidddle - 1, endmidddle);
// leftlinefuzhi(parameterA, parameterB, threeleftdown[0] - 1, 50);
// for (i = 0; i < 60; i++)
// {
// if (R_black[i] > 60) R_black[i] = 0;
// }
// for (j = 0; j < 70; j++)
// {
// LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
// }
// for (i = threeleftdown[0] - 1; i < 60; i++)
// {
// if (LCenter[i] > 93) LCenter[i] = 93;
// }
//
// }
// if (modetype ==8|| modetype==11) //
// {
// int endmidddle = 0;
// for (i = 0; i < 60; i++)
// {
// if (Pixels[i][93] == 0)
// {
// endmidddle = i;
// break;
// }
// }
// if (endmidddle <= 1) endmidddle = 1;
// L_black[endmidddle - 1] = 93;
// L_black[endmidddle] = 90;
// advanced_regression(1,0,1, endmidddle - 1, endmidddle);
// leftlinefuzhi(parameterA, parameterB, 0, 50);
// for (i = 0; i < 60; i++)
// {
// if (R_black[i] > 60) R_black[i] = 0;
// }
// for (j = 0; j < 70; j++)
// {
// LCenter[j] = (unsigned char)((L_black[j] + R_black[j]) / 2);
// }
// for (i = 0; i < 60; i++)
// {
// if (LCenter[i] > 93) LCenter[i] = 93;
// }
// }
//
// }
// //============================================三岔路口操作结束=======================================
//// //======================================弯道找拐点开始=============================================迟点调
//// void findwanguaidian()
//// {
//// int j;
//// flag_left_down_point_wandao = 0;
//// flag_right_down_point_wandao = 0;
//// right_turn_down_wandao[0] = 0;
//// right_turn_down_wandao[1] = 0;
//// left_turn_down_wandao[0] = 0;
//// left_turn_down_wandao[1] = 0;
//// for (j = 2; j < 49; j++)
//// {
//// if (j < shizimax) j = shizimax;
//// if (j < 42)
//// {
//// if (flag_left_down_point_wandao == 0 && My_Abs(L_black[j - 1], L_black[j - 2]) <= 3 && My_Abs(L_black[j], L_black[j - 1]) <= 3
//// && (L_black[j + 1] - L_black[j] >= 0) && (L_black[j + 2] - L_black[j] >= 1) && leftflag[j - 2] == 1 && leftflag[j - 1] == 1 && leftflag[j] == 1)
//// {
//// left_turn_down_wandao[0] = j + 1;//数组里面没有第0行
//// left_turn_down_wandao[1] = L_black[j];
//// flag_left_down_point_wandao = 1;
////
//// }
//// if (flag_right_down_point_wandao == 0 && My_Abs(R_black[j - 1], R_black[j - 2]) <= 3 && My_Abs(R_black[j], R_black[j - 1]) <= 3
//// && (R_black[j + 1] - R_black[j] <= 0) && (R_black[j + 2] - R_black[j] <= -1)
//// && rightflag[j - 2] == 1 && rightflag[j - 1] == 1 && rightflag[j] == 1)
//// {
//// right_turn_down_wandao[0] = j + 1;
//// right_turn_down_wandao[1] = R_black[j];
//// flag_right_down_point_wandao = 1;
////
//// }
//// }
//// }
//// wanguaidianmin = 0;
//// if (flag_left_down_point_wandao == 1 && flag_right_down_point_wandao == 0) //只找到左下拐点
//// {
//// wanguaidianmin = left_turn_down_wandao[0];
//// }
//// if (flag_left_down_point_wandao == 0 && flag_right_down_point_wandao == 1) //只找到左下拐点
//// {
//// wanguaidianmin = right_turn_down_wandao[0];
//// }
//// if (flag_left_down_point_wandao == 1 && flag_right_down_point_wandao == 1) //只找到左下拐点
//// {
//// if (left_turn_down_wandao[0] >= right_turn_down_wandao[0]) wanguaidianmin = right_turn_down_wandao[0];
//// if (left_turn_down_wandao[0] <right_turn_down_wandao[0]) wanguaidianmin = left_turn_down_wandao[0];
//// }
//// }
//// //======================================弯道找拐点结束==============================================
////
////
//// //=========================================问号弯================================================
//// void wenhaowan()
//// {
//// if (trueshortflag == 0 && truelongflag == 0)
//// {
//// countwenhaowan = 0;
//// truewenhaowanflag = 0;
//// truewenhaowanhang = 0;
//// }
//// if (truewenhaowanflag==0&&(trueshortflag == 1|| truelongflag==1)&& wanguaidianmin != 0&&firstmid==0)
//// {
//// int hang;
//// int end=0;
//// for (hang = 1; hang < 69; hang++) //去掉杂点
//// {
////
//// if (LCenter[hang] == 0 && LCenter[hang + 1] == 0)
//// {
////
//// end = hang - 1;
//// break;
//// }
//// if (My_Abs(LCenter[hang], LCenter[hang + 1]) > 20 && hang > 5)
//// {
////
//// end = hang - 1;
//// break;
//// }
//// }
////
//// leftwenhaoflag = 0;
//// rightwenhaoflag = 0;
//// if (end > 43) end = 43;
//// if (end-2 > wanguaidianmin&& wanguaidianmin!=0)
//// {
//// find_rightwenhao_point(wanguaidianmin, end - 2);
//// find_leftwenhao_point(wanguaidianmin, end - 2);
//// }
//// if ((rightwenhaoflag == 1 || leftwenhaoflag == 1)&&end<=42)
//// {
//// countwenhaowan =countwenhaowan + 1;
//// }
//// if (countwenhaowan >= 4)
//// {
//// countwenhaowan = 0;
//// truewenhaowanflag = 1;
//// }
//// }
////
//// if (truewenhaowanflag == 1)
//// {
//// int hang;
//// int end = 0;
//// for (hang = 1; hang < 69; hang++) //去掉杂点
//// {
////
//// if (LCenter[hang] == 0 && LCenter[hang + 1] == 0)
//// {
////
//// end = hang - 1;
//// break;
//// }
//// if (My_Abs(LCenter[hang], LCenter[hang + 1]) > 20 && hang > 5)
//// {
////
//// end = hang - 1;
//// break;
//// }
//// }
////
//// if (end > 15)
//// {
//// leftwenhaoflag = 0;
//// rightwenhaoflag = 0;
//// if (end > 42) end = 42;
//// find_rightwenhao_point(end-15, end - 3);
//// find_leftwenhao_point(end-15, end - 3);
//// if(rightwenhaoflag == 1 && leftwenhaoflag == 0)
//// {
//// truewenhaowanhang = right_wenhao[0];
////
//// }
//// if (rightwenhaoflag == 0 && leftwenhaoflag == 1)
//// {
//// truewenhaowanhang = left_wenhao[0];
////
//// }
//// if (rightwenhaoflag == 1 && leftwenhaoflag == 1)
//// {
//// if (right_wenhao[0] >= left_wenhao[0]) truewenhaowanhang = left_wenhao[0];
//// if (right_wenhao[0] <= left_wenhao[0]) truewenhaowanhang = right_wenhao[0];
//// }
//// }
//// }
//// }
//// //=========================================问号弯================================================
//
//
// //==============================================长直道===========================================
// void longstraight()
// {
// float kfirst = 0;
// float ksecond = 0;
// float kerror = 0;
// int longwrong = 0;
// int longbreak = 0;
// int longpianwrong = 0;
// int lrwrong = 0;
// if (turepodaoflag == 0 && (countpodao == 0 || countpodao > 15) && rukuflag == 0 && ((huandao_memory == 0 || huandao_memory == 8) && (huandao_memoryforleft == 0 || huandao_memoryforleft == 8)))
// {
// //SetText("进入这里1");
// int end = 0;
// int hang;
//
// for (hang =15; hang < 69; hang++) //去掉杂点
// {
//
// if ((R_black[hang] == 0 && R_black[hang + 1] == 0))
// {
//
// end = hang - 1;
// break;
// }
// }
//
// if (truelongflag == 0&& end>36)
// {
// int i = 0;
//
// truelongflag = 0;
// kfirst = 0;
// ksecond = 0;
// kerror = 0;
// longwrong = 0;
// longbreak = 0;
// longpianwrong = 0;
//
//
// overlong = 0;
// overlongwrong = 0;
// if (lastturesmallsflag == 1 && turesmallsflag == 0) { stopsmalls = 1; };//小s内部不判断
// if (stopsmalls == 1)
// {
// stopsmalls = 0;
// countstopsmalls = 1;
// }
// if (countstopsmalls != 0)
// { countstopsmalls++; }
// if (countstopsmalls == 10) { countstopsmalls = 0; }
//
// if (countstopsmalls == 0)
// {
//
// for (i = 0; i <= 30; i++)
// {
// if (LCenter[i] < 20 || LCenter[i] > 160)
// {
// longpianwrong = 1;
//
// break;
// }
// }
// int breakwrong = 0;
// for (i = 15; i <= 38; i++)
// {
// if (My_Abs(LCenter[i], LCenter[i - 1]) > 4)
// {
// breakwrong = 1;
//
// break;
// }
// }
//
// if (breakwrong == 0 && turesmallsflag == 0 && longpianwrong == 0 )
// {
// i = 0;
// advanced_regression(0, 12, 16, 20, 24);
// kfirst = parameterB;
//
// advanced_regression(0, 28, 32, 36, 41);
// ksecond = parameterB;
//
// if (FMy_Abs(0, kfirst) >= 0.7|| FMy_Abs(0, ksecond) >= 0.7)
// {
// longwrong = 1;
// longflag = 0;
//
// }
//
// if ( (secondmid==0&&se_thrmid==0&& cumulants1 > 40)||((secondmid==1|| se_thrmid==1) && cumulants1>=110))
// {
// longwrong = 1;
// longflag = 0;
//
// }
// if (break_hangshu<=41)
// {
// longflag = 0;
//
// }
// if (My_Abs(LCenter[41], LCenter[40]) >=3 )
// {
// longflag = 0;
//
// }
// kerror = FMy_Abs(kfirst, ksecond);
//
// if (longwrong == 0)
// {
// if (kerror >= 0.4) { }
// if (kerror <= 0.4 && overlongwrong == 0) longflag = longflag + 1;
// if (kerror > 0.4 && overlongwrong == 1) { longflag = 0; }
// if (longflag >= 2) { longflag = 3; truelongflag = 1; trueshortflag = 0; }
// if (longflag <2) truelongflag = 0;
// if (wanguaidianmin != 0 && wanguaidianmin <= 37) longflag = 0;
// }
// }
//
//
// }
// }
// if (truelongflag == 1)
// {
// int i = 0;
// truelongflag = 0;
// kfirst = 0;
// ksecond = 0;
// kerror = 0;
// longwrong = 0;
// longbreak = 0;
// longpianwrong = 0;
// lrwrong = 0;
//
// overlong = 0;
// overlongwrong = 0;
//
// for (i = 0; i <= 30; i++)
// {
// if (LCenter[i] < 20 || LCenter[i] > 160)
// {
// longpianwrong = 1;
//
// break;
// }
// }
//
// if ( longpianwrong == 0 && lrwrong == 0)
// {
// i = 0;
//
// advanced_regression(0, 8, 12, 14, 16);
// kfirst = parameterB;
// turedian = 0;
// if (truewenhaowanflag==1&& truewenhaowanhang!=0)
// {
// for (i = truewenhaowanhang ; i >= 16; i = i - 1)
// {
// advanced_regression(0, 12, 12 + ((i - 12) / 3), 12 + ((i - 12) / 3 * 2), 12 + (i - 12)); //(i-20)/4
// ksecond = parameterB;
//
// kerror = FMy_Abs(kfirst, ksecond);
// if (kerror <= 0.5 && FMy_Abs(0, ksecond) <= 1) { turedian = i; break; }
// }
//
// }
// else
// {
// if (wanguaidianmin != 0 && break_hangshu >= wanguaidianmin + 3)
// {
// for (i = wanguaidianmin ; i >= 16; i = i - 1)
// {
// advanced_regression(0, 12, 12 + ((i - 12) / 3), 12 + ((i - 12) / 3 * 2), 12 + (i - 12)); //(i-20)/4
// ksecond = parameterB;
//
// kerror = FMy_Abs(kfirst, ksecond);
// if (kerror <= 1 && FMy_Abs(0, ksecond) <= 1) { turedian = i; break; }
// }
//
// }
// else
// {
// for (i = break_hangshu - 3; i >= 16; i = i - 1)
// {
// advanced_regression(0, 12, 12 + ((i - 12) / 3), 12 + ((i - 12) / 3 * 2), 12 + (i - 12)); //(i-20)/4
// ksecond = parameterB;
//
// kerror = FMy_Abs(kfirst, ksecond);
// if (kerror <= 1 && FMy_Abs(0, ksecond) <= 1) { turedian = i; break; }
// }
// }
// }
//
// if (turedian != 0)
// {
// if (turedian > 34) { truelongflag = 1; turelongtype1 = 1; turelongtype2 = 0; turelongtype3 = 0; }
// if (turedian <= 34&& turedian>30) { truelongflag = 1; turelongtype1 = 0; turelongtype2 = 1; turelongtype3 = 0; }
// if (turedian <= 30 && turedian > 16) { truelongflag = 1; turelongtype1 = 0; turelongtype2 = 0; turelongtype3 = 1; }
// if (turedian <= 16) { truelongflag = 0; turelongtype1 = 0; turelongtype2 = 0; turelongtype3 = 0; }
// }
// if (FMy_Abs(0, kfirst) >= 1.2 || FMy_Abs(0, ksecond) >= 1.2) { truelongflag = 0; turelongtype1 = 0; turelongtype2 = 0; turelongtype3 = 0; }
// if (turedian == 0) { truelongflag = 0; turelongtype1 = 0; turelongtype2 = 0; turelongtype3 = 0; }
//
//
// turedian = 0;
// }
//
// }
// lasttruelong2 = truelong2;
// if (truelongflag == 0) { turelongtype1 = 0; turelongtype2 = 0; turelongtype3 = 0; }
//
// }
// //if (lastturelongtype3 == 1 && turelongtype1 == 1) { truelongflag = 0; turelongtype1 = 0; turelongtype2 = 0; turelongtype3 = 0; }
// //lastturelongtype3 = turelongtype3;
// // if (lasttruelongflag == 1 && truelongflag == 0 ) countlongku = 1;
// // if (countlongku != 0) countlongku = countlongku + 1;
// // if (countlongku == 10) countlongku = 0;
// // if (countlongku != 0 && rukuflag == 1) downdown = 1;
// // if (rukuflag == 2) downdown = 0;
// if (truelongflag == 0) { turelongtype1 = 0; turelongtype2 = 0; turelongtype3 = 0; }
// lasttruelongflag = truelongflag;
//
// }
// //==============================================长直道===========================================
//
//
//
//
//
//
// //=========================================短直道===================================================
//
// void shortst()
// {
//
//
// int i;
// shortpianwrong = 0;
// shortpianwrong = 0;
// int end = 0;
// int hang;
// float kfirst=0;
// float ksecond=0;
// float kerror=0;
// for (hang = 1; hang < 69; hang++) //去掉杂点
// {
//
// if ((L_black[hang] == 0 && L_black[hang + 1] == 0))
// {
//
// end = hang - 1;
//
// break;
// }
//
// }
//
// if (trueshortflag == 0&&turepodaoflag == 0 && (countpodao == 0 || countpodao > 3) && rukuflag == 0 && ((huandao_memory == 0|| huandao_memory ==8) && (huandao_memoryforleft == 0|| huandao_memoryforleft == 8))&& truelongflag==0 && turesmallsflag == 0)
// {
//
// for (i = 0; i <= 30; i++)
// {
// if (LCenter[i] < 20 || LCenter[i] > 160)
// {
// shortpianwrong = 1;
//
// break;
// }
// }
// for (i = 6; i <= end - 8; i++)
// {
// if (My_Abs(LCenter[i], LCenter[i - 1]) > 4)
// {
// shortpianwrong = 1;
//
// break;
// }
// }
// shortwrong = 0;
// advanced_regression(0, 4,8,12,16);
// kfirst = parameterB;
//
// advanced_regression(0, 18, 22, 26, 30);
// ksecond = parameterB;
//
// if (FMy_Abs(0, kfirst) >= 0.7 || FMy_Abs(0, ksecond) >= 0.7) { shortwrong = 1; }
// kerror = FMy_Abs(kfirst, ksecond);
//
// if (kerror > 0.3) { }
// if (kerror <= 0.3) shortflag = shortflag + 1;
// if (kerror > 0.3 ) { shortflag = 0; }
// if(shortwrong==1) { shortflag = 0; }
// if (shortflag >= 2) { shortflag =2; trueshortflag = 1; }
// if (shortflag < 2) trueshortflag = 0;
//
// }
// if (trueshortflag == 1)
// {
//
// advanced_regression(0, 8, 12, 14, 16);
// kfirst = parameterB;
//
//
// turedian = 0;
// if (truewenhaowanflag==1&&truewenhaowanhang!=0)
// {
// for (i = truewenhaowanhang; i >= 14; i = i - 1)
// {
// advanced_regression(0, 12, 12 + ((i - 12) / 3), 12 + ((i - 12) / 3 * 2), 12 + (i - 12)); //(i-20)/4
// ksecond = parameterB;
// kerror = FMy_Abs(kfirst, ksecond);
//
// if (kerror <= 1 && FMy_Abs(0, ksecond) <= 1) { turedian = i; break; }
//
// }
//
// }
// else if (threemode == 1
// || threemode == 2 || threemode == 4 || threemode == 5 || threemode == 7 || threemode == 8 || threemode == 10 || threemode == 11)
// {
// for (i = bigup; i >= 14; i = i - 1)
// {
// advanced_regression(0, 12, 12 + ((i - 12) / 3), 12 + ((i - 12) / 3 * 2), 12 + (i - 12)); //(i-20)/4
// ksecond = parameterB;
// kerror = FMy_Abs(kfirst, ksecond);
//
// if (kerror <= 1 && FMy_Abs(0, ksecond) <= 1) { turedian = i; break; }
//
// }
//
// }
// else
// {
// for (i = end - 5; i >= 14; i = i - 1)
// {
//
// advanced_regression(0, 12, 12 + ((i - 12) / 3), 12 + ((i - 12) / 3 * 2), 12 + (i - 12)); //(i-20)/4
// ksecond = parameterB;
// kerror = FMy_Abs(kfirst, ksecond);
//
// if (kerror <= 0.5 && FMy_Abs(0, ksecond) <= 1) { turedian = i;break; }
//
// }
// }
// if (FMy_Abs(kfirst,0) > 1) turedian = 0;
// if (turedian >= 0)
// {
// if (turedian >= 30) { trueshortflag = 1; trueshorttype1 = 1; trueshorttype2 = 0; trueshorttype3 = 0;}
// if (turedian >25&& turedian <30 ) { trueshortflag = 1; trueshorttype1 = 0; trueshorttype2 = 1; trueshorttype3 = 0; }
// if (turedian >= 16 && turedian <= 25) { trueshortflag = 1; trueshorttype1 = 0; trueshorttype2 = 0; trueshorttype3 = 1; }
// if (turedian <16) { trueshortflag = 0; trueshorttype1 = 0; trueshorttype2 = 0; trueshorttype3 = 0; }
// if(FMy_Abs(kfirst,0)>1.2) { trueshortflag = 0; trueshorttype1 = 0; trueshorttype2 = 0; trueshorttype3 = 0; }
// }
//
// }
// if (turepodaoflag == 1 || truelongflag == 1 || rukuflag != 0 || (huandao_memory != 0&& huandao_memory != 8) || (huandao_memoryforleft != 0& huandao_memoryforleft != 8) || firstmid == 1)
// {
// trueshortflag = 0;
// trueshorttype1 = 0; trueshorttype1 = 0; trueshorttype3 = 0;
// }
//
// //if (lasttrueshorttype3 == 1 && trueshorttype1 == 1) { trueshortflag = 0; trueshorttype1 = 0; trueshorttype2 = 0; trueshorttype3 = 0; }
// //lasttrueshorttype3 = trueshorttype3;
//
// }
// //============================================短直道结束=================================================
//
//
//
// //========================================各种计数结束==================================================
//
// void everyjishu()
// {
//
//
// if (turepodaoflag == 0 && lasttruepodaoflag == 1) { podaojishu = podaojishu + 1; }
// if ((huandao_memory == 0 && lasthuandao_memory != 0) || (huandao_memoryforleft == 0 && lasthuandao_memoryforleft != 0)) { huandaojishu = huandaojishu + 1; }
//
// jishupodaotype = jishuhuandaot1ype[podaojishu];
// huandaotype = huandaot1ype[huandaojishu];
//
// if ((huandao_memory == 0 && lasthuandao_memory != 0) || (huandao_memoryforleft == 0 && lasthuandao_memoryforleft != 0)
// || (turepodaoflag == 0 && lasttruepodaoflag == 1)
// || (threemode == 3&& lastthreemode==2) || (threemode == 6 && lastthreemode == 5) || (threemode == 9 && lastthreemode == 8) || (threemode == 12 && lastthreemode == 11)
// )
// specialnumber = specialnumber + 1;
// if (firstku == 1) specialnumber = 0;
//
// if (secondmid == 1 || se_thrmid == 1) crossrecord = crossrecord + 1;
// if (secondmid == 0&& se_thrmid == 0) crossrecord = 0;
// if (crossrecord >= 3)
// {
// cross31 = 1;
// crossrecord = 0;
// }
//
// if (cross31 == 1&&(secondmid == 0 && se_thrmid == 0)) crossderecord = crossderecord + 1;
//
// if (crossderecord >= 5)
// {
// specialnumber = specialnumber + 1;
// crossrecord = 0;
// crossderecord = 0;
// cross31 = 0;
// }
//
// lastthreemode = threemode;
// lasthuandao_memory = huandao_memory;
// lasthuandao_memoryforleft = huandao_memoryforleft;
// lasttruepodaoflag = turepodaoflag;
// }
// //========================================各种计数结束==================================================
//
//
// //========================================延续函数开始===================================================
//
// void yanxu()
// {
// int hang;
// int leftguimax=0;
// int rightguimax=0;
// int guifin = 0;
// for (hang = 69; hang>=1; hang--) //去掉杂点
// {
//
// if (L_black[hang] == 0 && L_black[hang - 1] != 0)
// {
//
// leftguimax = hang - 1;
// break;
// }
// }
// for (hang = 69; hang >= 1; hang--) //去掉杂点
// {
//
// if (R_black[hang] == 0 && R_black[hang -1] != 0)
// {
//
// rightguimax = hang - 1;
// break;
// }
// }
// if ((leftguimax != 0 || rightguimax != 0)&& turepodaoflag==0)
// {
// if (rightguimax >= leftguimax) guifin = rightguimax;
// if (rightguimax < leftguimax) guifin = leftguimax;
//
// if (LCenter[guifin] > 93)
// {
// for (hang = guifin; hang <= 68; hang++)
// {
// LCenter[hang + 1] = 185;
// }
// }
// if (LCenter[guifin] < 93)
// {
// for (hang = guifin; hang <= 68; hang++)
// {
// LCenter[hang + 1] = 1;
// }
// }
// if (LCenter[guifin] == 93)
// {
// for (hang = guifin; hang <= 68; hang++)
// {
// LCenter[hang + 1] = 93;
// }
// }
// }
// }
//
// //==========================================延续函数结束================================================
//
SearchLine.h
#ifndef _SEARCH_LINE_H
#define _SEARCH_LINE_H
#include "zf_common_headfile.h"
#include "zf_common_typedef.h"
#define Search_Image_W MT9V03X_W //搜线宽度
#define Search_Image_H MT9V03X_H //搜线高度
#define BLACKPOINT 90 //黑点值
#define WHITEMAXMUL 11 //白点最大值基于参考点的放大倍数
#define WHITEMINMUL 7 //白点最小值基于参考点的放大倍数
#define REFRENCEROW 5 //参考点统计行数
#define SEARCHRANGE 20 //搜线半径
#define STOPROW 3 //搜线停止行
#define CONTRASTOFFSET 3 //搜线对比偏移
extern uint8 Reference_Point; //动态参考点
extern uint8 Reference_Col ; //动态参考列
extern uint8 White_Max_Point; //动态白点最大值
extern uint8 White_Min_Point; //动态白点最小值
extern uint8 Reference_Contrast_Ratio; //参考对比度
extern uint8 Contrast_Ratio[Search_Image_H]; //各行动态对比度
extern uint8 Left_Edge_Line[Search_Image_H]; //左右边界
extern uint8 Right_Edge_Line[Search_Image_H];
extern uint8 midline_Black[Search_Image_H]; //赛道中线
extern uint8 Remote_Distance[Search_Image_W]; //白点远端距离
void Get_Reference_Point(void);
void Search_Reference_Col(void);
void Search_line(void);
void Midline_Black(void);
int Find_extreme_value(uint8 *va , uint8 num0 ,uint8 num1 ,uint8 model);
#endif
SearchLine.c
#include "SearchLine.h"
uint8 Reference_Point = 0; // 动态参考点
uint8 Reference_Col = 0; // 动态参考列
uint8 White_Max_Point = 0; // 动态白点最大值
uint8 White_Min_Point = 0; // 动态白点最小值
uint8 Reference_Contrast_Ratio = 20; // 参考对比度
uint8 Remote_Distance[Search_Image_W] ={ 0 }; // 白点远端距离
uint8 Left_Edge_Line[Search_Image_H] ={ 0 }; // 左边界
uint8 Right_Edge_Line[Search_Image_H] ={ 0 }; // 右边界
uint8 midline_Black[Search_Image_H] ={ 0 }; //中线
/*******************************************************************************
* 函 数 名 * : LimitDouMid
* 函数功能 * : 将value限幅到limit1与limit2之间
*******************************************************************************/
int32 LimitDouMid(int32 value,int32 limit1,int32 limit2)
{
int32 buf = 0;
if(limit1 > limit2) // 要求limit1 < limit2
{
buf = limit1;
limit1 = limit2;
limit2 = buf;
}
if(value < limit1) return limit1;
else if(value > limit2) return limit2;
else return value;
}
/*******************************************************************************
* 函 数 名 * : Find_extreme_value
* 函数功能 * : 返回一维数组最大值或最小值
* *va :数组指针
* num0 :起始点
* num1 :结束点
* model : 1 返回最大值 0 返回最小值
*******************************************************************************/
int Find_extreme_value(uint8 *va , uint8 num0 ,uint8 num1 ,uint8 model)
{
uint8 i = 0, temp = 0, temp1 = 0, temp2 = 0, value = 0;
if(num0 > num1){
temp1 = num0 - num1;
temp2 = num1;
va += num0;
value = *va;
if(model)
{
for(i = 0; i <= temp1; i ++){
temp = *(va - i);
if(temp > value) { temp2 = num0 - i; value = temp; }
}
}
else {
for(i = 0; i <= temp1; i ++){
temp = *(va - i);
if(temp < value) { temp2 = num0 - i; value = temp; }
}
}
}
else{
temp1 = num1 - num0;
temp2 = num0;
va += num0;
value = *va;
if(model)
{
for(i = 0; i <= temp1; i ++){
temp = *(va + i);
if(temp > value) { temp2 = i + num0; value = temp; }
}
}
else {
for(i = 0; i <= temp1; i ++){
temp = *(va + i);
if(temp < value) { temp2 = i + num0; value = temp; }
}
}
}
return temp2;
}
// 获取图像参考点，白点最大值，白点最小值
void Get_Reference_Point(void)
{
uint8 *p = mt9v03x_image[Search_Image_H-REFRENCEROW]; // 图像数组指针指向待统计的起始点
uint16 temp = 0; // 保存统计点总数量
uint32 temp1 = 0; // 保存所有统计点加起来的和
temp = REFRENCEROW * Search_Image_W; // 计算待统计点总数量
for(int i = 0; i < temp; i ++) temp1 += *(p + i); // 统计点求和
Reference_Point = (uint8)(temp1 / temp); // 计算点平均值，作为本幅图像的参考点
White_Max_Point = (uint8)LimitDouMid((int32)Reference_Point * WHITEMAXMUL / 10, BLACKPOINT, 255); // 根据参考点计算白点最大值
White_Min_Point = (uint8)LimitDouMid((int32)Reference_Point * WHITEMINMUL / 10, BLACKPOINT, 255); // 根据参考点计算白点最小值
}
// 搜索每列白点距离及图像参考列，便于搜索边线
void Search_Reference_Col(void)
{
int col, row;
uint8 temp1 = 0, temp2 = 0;
int temp3 = 0;
uint8 *p = mt9v03x_image[0]; // 图像数组指针
for(col = 0; col < Search_Image_W; col ++)
{
for(row = Search_Image_H - 1; row > STOPROW; row --){
temp1 = *(p + row*Search_Image_W + col); // 获取当前点灰度值
temp2 = *(p + (row - STOPROW) * Search_Image_W + col); // 获取对比点灰度值
if(temp2 > White_Max_Point) continue; // 判断对比点是否为白点 若为白点则直接跳过
if(temp1 < White_Min_Point) { Remote_Distance[col] = row - 1; break; } // 判断当前点是否为黑点 若为黑点则不进行对比直接赋值
temp3 = (temp1 - temp2) * 200 / (temp1 + temp2); // 计算对比度
// 判断对比度是否大于阈值 如果大于阈值则认为搜索到边界 或者已经搜索到该行最边界，则直接赋值边界
if(temp3 > Reference_Contrast_Ratio || row == STOPROW + 1){Remote_Distance[col] = row - 1; break;}
}
}
Reference_Col = (uint8)Find_extreme_value(Remote_Distance, 10, Search_Image_W - 10, 0); // 求出最远的白点列作为参考列
Reference_Col = (uint8)LimitDouMid(Reference_Col, 1, Search_Image_W - 2); // 参考列限幅 防止数据溢出
}
//搜索赛道边界
void Search_line(void)
{
uint8 *p = mt9v03x_image[0]; // 图像数组指针
uint8 row_max = Search_Image_H-STOPROW; // 行最大值
uint8 row_min = STOPROW; // 行最小值
uint8 col_max = Search_Image_W-3; // 列最大值
uint8 col_min = 3; // 列最小值
int16 leftstartcol = Reference_Col; // 搜线左起始列
int16 rightstartcol = Reference_Col; // 搜线右起始列
int16 leftendcol = 0; // 搜线左终止列
int16 rightendcol = Search_Image_W; // 搜线右终止列
uint8 temp1 = 0, temp2 = 0; // 临时变量 用于存储图像数据
int temp3 = 0; // 临时变量 用于存储对比度
int leftstop = 0, rightstop = 0, stoppoint = 0; // 搜线自锁变量
int col, row;
for(row = row_max; row > row_min; row --){
Left_Edge_Line[row] = col_min - CONTRASTOFFSET;
Right_Edge_Line[row] = col_max + CONTRASTOFFSET;
}
for(row = row_max; row > row_min; row --){
p = &mt9v03x_image[row][0]; // 获取本行起点位置指针
if(!leftstop){
for(col = leftstartcol; col >= leftendcol; col --){
temp1 = *(p + col); //获取当前点灰度值
temp2 = *(p + col - CONTRASTOFFSET); //获取对比点灰度值
//判断参考列是否为黑点 若为黑点则放弃搜线
if(temp1 < White_Min_Point && col == leftstartcol && leftstartcol == Reference_Col){
leftstop = 1; //线搜索自锁 不在进行左边线搜索
for(stoppoint = row; stoppoint >= 0; stoppoint --) Left_Edge_Line[stoppoint] = col_min; //清除剩余的无效左边线
break;
}
//判断行搜索起始点是否为黑点 若为黑点则重新确立起始范围
if((temp1 < White_Min_Point && col == leftstartcol) || (leftstartcol != Reference_Col && col == col_min + 1)){
if(leftstartcol < Reference_Col){ col = Reference_Col; leftstartcol = Reference_Col; }
}
//判断行搜索终止点是否为白点 若为白点则重新确立结束范围
if(temp1 > White_Min_Point && col == leftendcol){
if(leftendcol > col_min){ leftendcol = col_min; }
}
//判断当前点是否为黑点 若为黑点则不进行对比直接赋值
if(temp1 < White_Min_Point && leftstartcol == Reference_Col){ Left_Edge_Line[row] = (uint8)col; break;}
//判断对比点是否为白点 若为白点则直接跳过
if(temp2 > White_Max_Point) continue;
//计算对比度
temp3 = (temp1 - temp2) * 200 / (temp1 + temp2);
//判断对比度是否大于阈值 如果大于阈值则认为是行边界 或者已经搜索到该行最边界，则直接赋值行边界
if(temp3 > Reference_Contrast_Ratio || col == col_min + 1){
Left_Edge_Line[row] = col - CONTRASTOFFSET+1; //保存当前行左边界
//刷新搜线范围
leftstartcol = (uint8)LimitDouMid(col + SEARCHRANGE, col, Reference_Col);
leftendcol = (uint8)LimitDouMid(col - SEARCHRANGE, col_min, col);
break;
}
}
}
if(!rightstop){
for(col = rightstartcol; col <= rightendcol; col ++){
temp1 = *(p + col); //获取当前点灰度值
temp2 = *(p + col + CONTRASTOFFSET); //获取对比点灰度值
//判断参考列是否为黑点 若为黑点则放弃搜线
if(temp1 < White_Min_Point && col == rightstartcol && rightstartcol == Reference_Col){
rightstop = 1; //线搜索自锁 不再进行右边线搜索
for(stoppoint = row; stoppoint >= 0; stoppoint --) Right_Edge_Line[stoppoint] = col_max; //清除剩余的无效右边线
break;
}
//判断行搜索起始点是否为黑点 若为黑点则重新确立搜线范围
if((temp1 < White_Min_Point && col == rightstartcol) || (rightstartcol != Reference_Col && col == col_max - 1)){
if(rightstartcol < Reference_Col){col = Reference_Col; rightstartcol = Reference_Col;}
}
//判断行搜索终止点是否为白点 若为白点则重新确立结束范围
if(temp1 < White_Max_Point && col == rightendcol){
if(rightendcol > col_max) rightendcol = col_max;
}
//判断当前点是否为黑点 若为黑点则不进行对比直接赋值
if(temp1 < White_Min_Point && rightstartcol == Reference_Col){ Right_Edge_Line[row] = (uint8)col; break;}
//判断待对比点是否为白点 若为白点则直接跳过
if(temp2 > White_Max_Point) continue;
//计算对比度
temp3 = (temp1 - temp2) * 200 / (temp1 + temp2);
//判断对比度是否大于阈值 如果大于阈值则认为是行边界 或者已经搜索到该行最边界，则直接赋值行边界
if(temp3 > Reference_Contrast_Ratio || col == col_max - 1){
Right_Edge_Line[row] = col + CONTRASTOFFSET - 1; //保存当前行右边界
//刷新搜线范围
rightstartcol = (uint8)LimitDouMid(col - SEARCHRANGE, Reference_Col, col);
rightendcol = (uint8)LimitDouMid(col + SEARCHRANGE, col, col_max);
break;
}
}
}
}
}
void Midline_Black()
{
uint8 midline_Black[Search_Image_H];
for(int8 i=0;i<Search_Image_H;i++)
{
midline_Black[i] =(Left_Edge_Line[i]+Right_Edge_Line[i])/2;//取中线
}
}
/************************************线性回归计算中线斜率************************************/
// y = Ax+B
int8 regression_(int startline,int endline)
{
int8 i=0,SumX=0,SumY=0,SumLines = 0;
int8 SumUp=0,SumDown=0,avrX=0,avrY=0,B,A;
SumLines=endline-startline; // startline 为开始行， //endline 结束行 //SumLines
for(i=startline;i<endline;i++)
{
SumX+=i;
SumY+=midline_Black[i]; //这里Middle_black为存放中线的数组
}
avrX=SumX*100/SumLines; //X的平均值
avrY=SumY*100/SumLines; //Y的平均值
SumUp=0;
SumDown=0;
for(i=startline;i<endline;i++)
{
SumUp+=(midline_Black[i]-avrY)*(i-avrX);
SumDown+=(i-avrX)*(i-avrX);
}
if(SumDown==0)
B=0;
else
B=(int8)(SumUp/SumDown)/100;
A=(SumY-B*SumX)/SumLines; //截距
return B; //返回斜率
}Camera_Process.h
/*
* Camera_Process.h
*
* Created on: 2023年3月4日
* Author: 同位素
*/
#ifndef CODE_CAMERA_PROCESS_H_
#define CODE_CAMERA_PROCESS_H_
#include "zf_common_headfile.h"
#endif /* CODE_CAMERA_PROCESS_H_ */Camera_Process.c
/*
* Camera_Process.c
*
* Created on: 2023年3月4日
* Author: 同位素
*/
//图像处理开始
void ImageProcess()
{
if(1)
{
// zhongxian();
zhongxian(); //基本扫线
continuepanduan(); //左右线连续
continuepanduanforleft();
cumulants();
huandaofuzhu(); //环岛辅助
cakkuandu(); //计算宽度
//calend();
podaojudge(); //坡道 需要复制
mynewleftku();
juge_left_succesive_and_if_k_limit(); //右环岛 加
juge_left_succesive_and_if_k_limitforleft(); //左环岛 加
threeroadfuzhu();
buzhongxian4(); //斜入十字 加
buzhongxian2();//正入十字 加
buzhongxian2_2();
mysmalls(); //加
findmiddle();
//bushizidan();//正入单点补十字 加
threeroad();
findwanguaidian();
wenhaowan();
longstraight();
shortst();
straightjudge();
everyjishu();
yanxu();
}
}
//图像处理结束Binarization.h
/*
* Binarization.h
*
* Created on: 2023年2月25日
* Author: 同位素
*/
#ifndef CODE_BINARIZATION_H_
#define CODE_BINARIZATION_H_
#include "zf_common_headfile.h"
extern uint8 mt9v03x_image_NEW[MT9V03X_H][MT9V03X_W];
extern uint8 mt9v03x_image_project[MT9V03X_H][MT9V03X_W];
extern uint8 Pixels[MT9V03X_H][MT9V03X_W];
void binarization(uint8 value);
void binarization_2(uint8 value);
void exercise(void);
void Bin_Image_Filter (void);
uint8 ostu_binaryzation(uint8 *p, uint8 height, uint8 width);
#endif /* CODE_BINARIZATION_H_ */Binarization.c
/*
* Binarization.c
*
* Created on: 2023年2月25日
* Author: 同位素
*/
#include "zf_common_headfile.h"
#include "zf_common_typedef.h"
#include "Binarization.h"
uint8 mt9v03x_image_NEW[MT9V03X_H][MT9V03X_W];
uint8 mt9v03x_image_project[MT9V03X_H][MT9V03X_W];
uint8 Pixels[MT9V03X_H][MT9V03X_W];
void binarization(uint8 value)//二值化显示图像
{ uint8 tempvalue;
for(uint8 i=0;i<MT9V03X_H;i++)
{
for(uint8 j=0;j<MT9V03X_W;j++)
{ tempvalue=mt9v03x_image_project[i][j];
if( tempvalue<=value)
{
mt9v03x_image_NEW[i][j]=0;
}
else
{
mt9v03x_image_NEW[i][j]=255;
}
}
}
}
//做个用来运算的图像数组，
void binarization_2(uint8 value)//二值化代码数组
{ uint8 tempvalue_2;
for(uint8 i=0;i<MT9V03X_H;i++)
{
for(uint8 j=0;j<MT9V03X_W;j++)
{ tempvalue_2=mt9v03x_image_project[i][j];
if( tempvalue_2<=value)
{
Pixels[i][j]=0;
}
else
{
Pixels[i][j]=1;//只是这里改变
}
}
}
}
void exercise()//数组颠倒
{
for(int j=MT9V03X_H;j>=0;j--)
{
for(int i=MT9V03X_W;i>=0;i--)
{
mt9v03x_image_project[MT9V03X_H-j][MT9V03X_W-i]=mt9v03x_image[j][i];
}
}
}
// An highlighted block
/*---------------------------------------------------------------
【函 数】Bin_Image_Filter
【功 能】过滤噪点
【参 数】无
【返 回 值】无
【注意事项】
----------------------------------------------------------------*/
void Bin_Image_Filter (void)
{
int16 nr; //行
int16 nc; //列
for (nr = 1; nr < MT9V03X_H - 1; nr++)
{
for (nc = 1; nc < MT9V03X_W - 1; nc = nc + 1)
{
if ((Pixels[nr][nc] == 0)
&& (Pixels[nr - 1][nc] + Pixels[nr + 1][nc] + Pixels[nr][nc + 1] + Pixels[nr][nc - 1] > 2))
{
Pixels[nr][nc] = 1;
}
else if ((Pixels[nr][nc] == 1)
&& (Pixels[nr - 1][nc] + Pixels[nr + 1][nc] + Pixels[nr][nc + 1] + Pixels[nr][nc - 1] < 2))
{
Pixels[nr][nc] = 0;
}
}
}
}
//-------------------------------------------------------------------------------------------------------------------
// @brief 大津法二值化
// @param *p 图像指针
// @param height 图像高度或者行数
// @param col 图像宽度或者列数
// @return 大津法阈值
// Sample usage: ostu_binaryzation(image[0],120,120)
//-------------------------------------------------------------------------------------------------------------------
uint8 ostu_binaryzation(uint8 *p, uint8 height, uint8 width)
{ uint8 back_mean_gray[256] = {0}; //GrayScale就是图像的量化精度，一般就是256
uint8 front_mean_gray[256] = {0};
uint16 back_count_array[256]={0};
float back_proportion_array[256]={0};
uint16 pixel_num = MT9V03X_H*MT9V03X_W;//像素数量图像长乘以宽
uint32 gray_sum = 0;
uint32 back_graysum = 0;
uint16 back_pixelsum = 0;
float class_variance=0;
float tmp_class_variance=0;
uint8 threshold=0;
uint16 tmp_count=0;
for(int i = 0;i<256;i++)
{
back_proportion_array[i] = 0;
back_count_array[i] = 0;
}
// memset(back_proportion_array, 0, sizeof(back_proportion_array));
// memset(back_proportion_count, 0, sizeof(back_proportion_count)); //C语言内置的将数组置0的程序，但试了一下好像还没上面的方法快
for(int j=0;j<pixel_num;j++)
{
gray_sum += *(p+j);
back_count_array[*(p+j)] += 1;
}
for(int i=0;i<256;i++)
{
back_graysum += back_count_array[i]*i;
back_pixelsum += back_count_array[i];
back_mean_gray[i] = back_graysum/back_pixelsum;
front_mean_gray[i] = (gray_sum - back_graysum)/(pixel_num - back_pixelsum);
back_proportion_array[i] = (float)back_pixelsum/pixel_num;
tmp_class_variance = back_proportion_array[i]*(1-back_proportion_array[i])*(back_mean_gray[i]-front_mean_gray[i])*(back_mean_gray[i]-front_mean_gray[i]);
if(tmp_class_variance>class_variance)
{
threshold = i;
class_variance = tmp_class_variance;
}
}
return threshold;
}
uint8 my_adapt_threshold(uint8 *image, uint16 col, uint16 row) //注意计算阈值的一定要是原图像
{
#define GrayScale 256
uint16 width = col;
uint16 height = row;
int pixelCount[GrayScale];
float pixelPro[GrayScale];
int i, j, pixelSum = width * height/4;
uint8 threshold = 0;
uint8* data = image; //指向像素数据的指针
for (i = 0; i < GrayScale; i++)
{
pixelCount[i] = 0;
pixelPro[i] = 0;
}
uint32 gray_sum=0;
//统计灰度级中每个像素在整幅图像中的个数
for (i = 0; i < height; i+=2)
{
for (j = 0; j < width; j+=2)
{
pixelCount[(int)data[i * width + j]]++; //将当前的点的像素值作为计数数组的下标
gray_sum+=(int)data[i * width + j]; //灰度值总和
}
}
//计算每个像素值的点在整幅图像中的比例
for (i = 0; i < GrayScale; i++)
{
pixelPro[i] = (float)pixelCount[i] / pixelSum;
}
//遍历灰度级[0,255]
float w0, w1, u0tmp, u1tmp, u0, u1, u, deltaTmp, deltaMax = 0;
w0 = w1 = u0tmp = u1tmp = u0 = u1 = u = deltaTmp = 0;
for (j = 0; j < GrayScale; j++)
{
w0 += pixelPro[j]; //背景部分每个灰度值的像素点所占比例之和 即背景部分的比例
u0tmp += j * pixelPro[j]; //背景部分 每个灰度值的点的比例 *灰度值
w1=1-w0;
u1tmp=gray_sum/pixelSum-u0tmp;
u0 = u0tmp / w0; //背景平均灰度
u1 = u1tmp / w1; //前景平均灰度
u = u0tmp + u1tmp; //全局平均灰度
deltaTmp = w0 * pow((u0 - u), 2) + w1 * pow((u1 - u), 2);
if (deltaTmp > deltaMax)
{
deltaMax = deltaTmp;
threshold = j;
}
if (deltaTmp < deltaMax)
{
break;
}
}
return threshold;
}写在后面
至于main函数这里就不做展示了，说来惭愧，博主到现在都没怎么弄明白面对双核处理器到底要怎么写main函数，不过有了前面的工具函数，基本上你的智能车之旅已经完成大多数了，弄明白main函数指日可待！
- 感谢你赐予我前进的力量
-
微信 
支付宝
赞赏者名单
因为你们的支持让我意识到写文章的价值🙏
本文是原创文章，采用 CC BY-NC-ND 4.0 协议，完整转载请注明来自 郭同学的笔记本
阅读建议
评论
匿名评论
隐私政策
你无需删除空行，直接评论以获取最佳展示效果
