2018 年第十三届全国大学生“恩智浦”杯智能汽车竞赛全国一等奖 - 匿名队伍.zip

#include "control.h" #include "NRF2401.H" #include "Wireless.h" #include "headfile.h" //变量定义 uint8_t chose_dajiaoline = 0;//打角行 uint8_t huan_line[8];//环岛打角行 uint8_t huancount=0;//环岛计数 uint8_t ruhuan_line_add;//入环增加的打角行 uint8_t dajiao_line=0;//设定的基础打角行 uint8_t podao_flag=0;//坡道标志 int dianboGeLi_count=0;//颠簸隔离 uint8_t dianbo_dajiaofield=0;//颠簸一段时间限制打角场数设定 uint8_t huan_ru_p[8];//入环p设定 uint8_t huan_chu_p[8];//出环p uint8_t huan_in_p[8];//环内p double chose_p=0, chose_d=0;//舵机pd float Long_Straight_P = 0.3, Long_Straight_D = 0.2;//长直道pd float Short_Straight_P = 0.6, Short_Straight_D = 0.5;//短直道pd float SS_P = 0.6, SS_D = 0.7;//小Spd float Curve_P = 0.9, Curve_D = 1.9;//弯道pd float Ramp_P = 0.6, Ramp_D = 0.4;//坡道pd uint8_t Ahuiche_mend_p=0, Ahuiche_recover_p=0,Ahuiche_mend_d=0, Ahuiche_recover_d=0;//起始会车区补线,恢复的p d uint8_t Bhuiche_mend_p=0, Bhuiche_recover_p=0,Bhuiche_mend_d=0, Bhuiche_recover_d=0;//中间会车区补线,恢复的p d uint8_t my_Element = 0, Last_my_Element = 0;//赛道类型 uint8_t die_dangche_Flag=0;//放弃会车 直接冲出去 uint8_t Realbegin_Count=0;//放弃会车 这个场数之后冲出去 int angleChange = 0;//舵机值变化的角度 int image_speedset = 0;//根据不同元素给出的设定速度 int speed_longstraight = 450;//长直道速度 uint16_t speed_shortstraight = 420; uint16_t speed_SS = 420, speed_ramp = 300, speed_normal = 360;//小S、坡道、弯道 速度 uint16_t motor_str_I = 12,motor_str_P = 15;//电机直道pi uint16_t motor_cur_I = 10,motor_cur_P = 10;//电机弯道pi uint8_t RW_flag = 0;//这次入弯减速是否有效 uint8_t Cut_Chang=0;//减速剩余场数 uint8_t Cut_finishFlag = 0;//减速完成标志位 uint8_t xiaopo_speed=0;//缓坡速度 uint16_t huan_size[8];//环岛加速 uint8_t CarFlag=2;//会车时前后车 0-前车 1-后车 int MiddleStop_Init=0;//中间会车几场停下来 int huichecount=0;//中间会车停车场数记录 int after_huiche_count=0;//会车之后记场 int EndStop_Init=0;//最后会车几场停下来 int tingchecount=0;//最后会车停车场数记录 uint8_t have_stop=0;//车已经停下来了 int stop_count=0;//最后一次会车完成之后准备停车记场 uint8_t RealStop_Count=0;//最后会车完成之后几场停下来 设定 uint8_t longtime_huiche_Flag=0;//双车模式 一辆车失败保成绩开关 uint8_t car1_lowspeed=0;//会车后车减速的大小 uint8_t motor_stop=0;//停车标志位 uint8_t dianbofield=0;//颠簸之后占空比输出限幅场数设定 int dianbospeed=0;//颠簸设定的占空比 int init_dianji_go=0;//电机的起始占空比 uint8_t guancha=0;//会车启动时直冲 该变量用于检测前车是否已停下 uint8_t my_Curve_Flag = 0;//确定赛道类型时使用的弯道标记 //会车相关变量 uint8_t huicheNum=0; uint8_t begin_stop=0; int Mid_huiche_Count=0; int Menu_Mid_huiche_Count=0; uint8_t tongshi_huiche=0; uint8_t test=0; int houche_huiche_count=0; int huiche_Menu_uwb=0; extern int last_distance[5]; int houche_huiche_Menu_count=0; int huicheyanshi_count=0; uint8_t huicheyanshi=0; uint8_t last_huicheyanshi=0; uint8_t Stop_Line_Huiche=0,huichejuli=0,huiche=0,xueruo=0,send_fail_count=0,send_fail_flag=0; uint8_t houchexueruo=0; int car0_stop_count=0; uint8_t hou_send=0; int End_huiche_count=0; int Menu_End_huiche_Count=0; int after_lasthuiche_count=0; //引用变量 //编码器 extern int Add_Pulse;//编码器转速和 //颠簸 extern uint8_t qi;//颠簸起 extern uint8_t dianbo ;//颠簸标志 extern int dianboGeLi;//颠簸隔离 extern uint8_t sure_huan_after_dianbo;//确认颠簸之后的环岛 extern uint8_t dianbo_zhichong;//颠簸直冲标志位 extern uint8_t dian_pan_huan; //坡道 extern int podaojichang;//坡道记场 extern int PanPoJishu;//坡道数量 extern uint8_t dian_pan_huan;//颠簸稳定状态 //图像 extern uint8_t RowNum,L_black[70],R_black[70],LCenter[70];//左右线 extern uint8_t L_init ,R_init ,R_init_record,L_init_record;//左右起始行 extern uint8_t Last_Line;//截断行 extern uint8_t mend_by_right;//疑似环岛借助正确线拉线 extern double Judge_Slope;//小草法斜率 extern uint8_t Judge_Memory;//小草法有效长度 extern uint8_t fixvalue[70];//正常赛道宽度 extern int CNT;//帧数记录 extern uint8_t Cross_Xie_Out_i, Cross_Zhong_Out_i, Cross_Zhi_Out_i;//十字标记 extern uint8_t xiaopo_jiansu;//缓坡 extern uint8_t small_s;//小S extern uint8_t SS_needCut;//小S需要减速 extern uint8_t small_sCanGo;//小S可以开始标志 //控制 extern int image_error, image_derror, image_lasterror;//偏差 extern double MotorFuzzyLP, MotorFuzzyLD, MotorFuzzyRP, MotorFuzzyRD;//计算得出的电机pd //会车区 extern uint8_t mend_by_xuxian;//会车区补线 extern uint8_t mend_recover;//会车处于恢复阶段 extern uint8_t xuxianjilu;//第几次判断到会车区 中间会车是1 extern uint8_t podaoF;//坡道开关 extern uint8_t dianboF;//颠簸开关 extern uint8_t huanline_second; extern uint8_t youDaSi; uint8_t huichequF = 0;//会车区检测开关 uint8_t huichejuli_cut=0; int zhidao_huiche_count=0; uint8_t last_CarFlag=2; extern uint8_t Judge_Start;//起始丢线 extern uint8_t SS_dajiao_line; extern uint8_t Car; //一段距离减速 //变量 //一段距离减速 //菜单 uint8_t AI_Element[8]={1,2,1,2,0,0,0,0};//1代表坡道 2代表颠簸 3会车区 4单纯减速 0关闭 uint8_t AI_Element_set[8] = {0,0,0,0,0,0,0,0};//人工智能是在那种元素之后记录的 0-环岛 1-坡道 2-会车区 uint8_t AI_num_set[8] = {0,0,0,0,0,0,0,0};//人工智能的元素数量设定 uint8_t cutCM_In[8] = {2,1,7,0,0}, cutCM_Out[8] = {4,4,4,0,0};//减速的路程 uint8_t podaoF_set = 0, dianboF_set = 0, huichequF_set = 0;//大开关 覆盖所有AI参数 uint8_t cutIsland_set[8] = {10,10,10,10,10,10,10,10}, cutRamp_set[8] = {10,10,10,10,10,10,10,10}, cutDashed_set[8] = {10,10,10,10,10,10,10,10}; uint8_t AI_CutCnt = 0;//第几个环岛 uint8_t AI_Use[8] = {0,0,0,0,0,0,0,0};//代表是否已经使用过这次人工智能减速 //uint16_t AI_SpeedSet[5] = {100,100,100,10,10};//减速的速度 uint64_t Island_distance[9] = {0, 100000, 100000, 100000, 100000, 100000, 100000, 100000, 100000};//每个环岛所在的距离 uint64_t Ramp_distance[9] = {0, 100000, 100000, 100000, 100000, 100000, 100000, 100000, 100000};//每个坡道所在的距离 uint64_t DashedField_distance = 100000;//虚线的距离 //状态 uint8_t AI_State = 0;//0-正常 1-减速限制中 uint8_t last_huancount = 0;//记录上一次环岛数量 判断是不是有环岛数量的变化 uint8_t last_PanPoJishu = 0;//记录上一次坡道数量 uint8_t last_dianbo = 0; int64_t encoderCNT = 0;//编码器计数和 uint16_t encoderCM = 0;//实际换算出来的米数（厘米） uint16_t AI_Speed = 100;//减速的速度 uint8_t AI_RecoveryCnt = 0;//记场恢复 10场恢复到正常 uint8_t AI_Distance_By = 0;//0-环岛 1-坡道 uint8_t Cut_AfterSS = 0, Cut_AfterIsland = 0;//小S、环岛 之后减速 uint8_t CurveFirst_ByIsland = 0;//环岛直入弯标记 uint8_t wandao_huiche_flag=0; //函数 extern int Mabs(int a,int b);//取绝对值最大的函数 int my_My_Abs1(int x) { if(x >= 0) return x; else return -x; } uint8_t my_My_Abs(int x) { if (x < 0) return (uint8_t)(-x); else return (uint8_t)x; } int My_Max(int x, int y) { if (x > y) return x; else return y; } //舵机控制部分 void Get_AngleLine()//确定打角行 { //先确定环岛的基础打角行 if((zuohuan == 2 || youhuan == 2)&&huanline_second>0)//入环 { chose_dajiaoline = huan_line[huancount] - ruhuan_line_add; } else if(zuohuan == 2 || youhuan == 2||zuohuan == 6 || youhuan == 6 || zuohuan == 3 || youhuan == 3 || youhuan == 5 || zuohuan == 5)//环内和出环 { chose_dajiaoline=huan_line[huancount]; } //处理环岛动态打角行 if(zuohuan == 2 || zuohuan == 3 || zuohuan == 5 || zuohuan == 6 || youhuan == 2 || youhuan == 3 || youhuan == 5 || youhuan == 6)//根据当前速度确定打角行 { if(Add_Pulse<600){chose_dajiaoline+=0;} else if(Add_Pulse<620){chose_dajiaoline+=0;} else if(Add_Pulse<640){chose_dajiaoline+=1;} else if(Add_Pulse<660){chose_dajiaoline+=1;} else if(Add_Pulse<680){chose_dajiaoline+=2;} else if(Add_Pulse<700){chose_dajiaoline+=2;} else if(Add_Pulse<720){chose_dajiaoline+=3;} else if(Add_Pulse<740){chose_dajiaoline+=3;} else if(Add_Pulse<760){chose_dajiaoline+=4;} else if(Add_Pulse<780){chose_dajiaoline+=4;} else if(Add_Pulse<800){chose_dajiaoline+=5;} else if(Add_Pulse<820){chose_dajiaoline+=5;} else if(Add_Pulse<840){chose_dajiaoline+=6;} else if(Add_Pulse<860){chose_dajiaoline+=6;} else if(Add_Pulse<880){chose_dajiaoline+=7;}