19届智能车镜头组c车模TC264迪克旋转队开源代码

#include "zf_common_headfile.h" #pragma section all "cpu0_dsram" // **************************** 代码区域 **************************** //标准赛宽 const uint8 Standard_Road_Wide[MT9V03X_H]= { 10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30, 32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62, 64, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84, 86, 88, 90, 92, 94, 96, 98,100,102,104,106,108, 110,112,114,116,118,120,122,124,126,128, 130,132,134,136,138,140,142,144,146,148 }; //权重 const uint8 Weight[MT9V03X_H]= { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, //图像最远端00 ——09 行权重 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, //图像最远端10 ——19 行权重 1, 1, 1, 1, 1, 1, 1, 3, 4, 5, //图像最远端20 ——29 行权重 6, 7, 9,11,13,15,17,19,20,20, //图像最远端30 ——39 行权重 19,17,15,13,11, 9, 7, 5, 3, 1, //图像最远端40 ——49 行权重 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, //图像最远端50 ——59 行权重 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, //图像最远端60 ——69 行权重 }; //数字图像数组 const uint8 Image_Flags[][9][8]={ { //0 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //1 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //2 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0X00,0X00,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //3 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0XFF,0XFF,0X00}, {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //4 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0XFF,0X00,0X00,0X00}, {0X00,0X00,0XFF,0X00,0XFF,0X00,0X00,0X00}, {0X00,0XFF,0X00,0X00,0XFF,0X00,0X00,0X00}, {0X00,0XFF,0X00,0X00,0XFF,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0XFF,0X00,0X00,0X00}, }, { //5 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //6 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //7 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //8 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //9 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, }; volatile int Left_Line[MT9V03X_H];//左边线数组 volatile int Right_Line[MT9V03X_H];//右边线数组 volatile int Mid_Line[MT9V03X_H];//中线数组 volatile int Road_Wide[MT9V03X_H];//赛宽数组 volatile int White_Column[MT9V03X_H];//每列白列长度 volatile int Search_Stop_Line;//搜索截止行,只记录长度，想要坐标需要用视野高度减去该值 volatile int Boundry_Start_Left;//左右边界起始点 volatile int Boundry_Start_Right;//第一个非丢线点,常规边界起始点 volatile int Left_Lost_Time;//边界丢线数 volatile int Right_Lost_Time; volatile int Both_Lost_Time;//两边同时丢线数 volatile int Longest_White_Column_Left[2]; //最长白列,[0]是最长白列的长度，也就是Search_Stop_Line搜索截止行，[1]是第某列 volatile int Longest_White_Column_Right[2];//最长白列,[0]是最长白列的长度，也就是Search_Stop_Line搜索截止行，[1]是第某列 volatile int Left_Lost_Flag[MT9V03X_H] ; //左丢线数组，丢线置1，没丢线置0 volatile int Right_Lost_Flag[MT9V03X_H]; //右丢线数组，丢线置1，没丢线置0 #define IMG_BLACK 0x00 #define IMG_WHITE 0xff /* 灵敏度 */ #define GYRO_SENS 1/16.4 #define ACCE_SENS 90.0/4096 #define DT 0.01 extern volatile float FJ_Pitch;//偏航角 extern volatile float FJ_Angle;//俯仰角 volatile float FJ_Angle = 0;//最后引出的变量 volatile float FJ_Pitch = 0;//最后引出的变量 volatile float FJ_PitchSpeed = 0,FJ_LastPitchSpeed