<code class="lang-cpp">#include "Map_all.h"
#include "MathMap.h"
#define PI 3.1415926525
#define FixConstant 1  //修正一米误差，左轮与右轮之比
#define Kp 1 //PID算法比例系数
#define Ki 1
#define Kd 1
   
bool TrunM;
extern bool FinishM; //in STM32Jack.c
double Fixvalue;
bool WaitM;
extern int MyA;
extern int MyX;
extern int MyY;
bool ImpactCM;
bool CrassM;
double ComModel_1[]={2,1.5,1,0.5,0,0.5,1,1.5};
double ComModel_2[]={1.5,2,1.5,1,0.5,0,0.5,1};
double ComModel_3[]={1,1.5,2,1.5,1,0.5,0,0.5};
double ComModel_4[]={0.5,1,1.5,2,1.5,1,0.5,0};
double ComModel_5[]={0,0.5,1,1.5,2,1.5,1,0.5};
double ComModel_6[]={0.5,0,0.5,1,1.5,2,1.5,1};
double ComModel_7[]={1,0.5,0,0.5,1,1.5,2,1.5};
double ComModel_8[]={1.5,1,0.5,0,0.5,1,1.5,2};
double Model[8][10];
//PID算法用
    double LE;
    double E;
    double IS;
    double RA;
    double Pv;
    double Cv;
//移动模式：
//  8  1  2
//  7     3
//  6  5  4
//  1为正北
//地图形式
//  (0,0)(0,1)(0,2)(0,3)……
//  (1,0)(1,1)(1,2)(1,2)……
//  (2,0)(2,1)(2,2)(2,3)……
//  ……
//标准直行函数
//输入参数：Number（前进格数）
//输出参数：无
void MoveTo(int Number){//no complete   need a stop mark 
  Moto_List New;
    New.DirectionLeft=1;
    New.DirectionRight=1;
    New.TimeLeft=(long)(1+Number)/(Wheellength*(0.9/32));
    New.TimeRight=(long)(1+Number)/(Wheellength*(0.9/32))+Fixvalue;
    MotoSetting(&New);
    while(FinishM!=true)
    FinishM=false;
}
//碰撞检测函数
//输入参数：无
//输出参数：无
void CheckImpact(void){
    int T_x;
    int T_y;
    if(FinishM==false){
        if(TrunM==false){
        if(MyA==1)
            {T_x=MyX-1;T_y=MyY;}
        else if(MyA==2)
            {T_x=MyX-1;T_y=MyY+1;}
        else if(MyA==3)
            {T_x=MyX;T_y=MyY+1;}
        else if(MyA==4)
            {T_x=MyX+1;T_y=MyY+1;}
        else if(MyA==5)
            {T_x=MyX+1;T_y=MyY;}
        else if(MyA==6)
            {T_x=MyX+1;T_y=MyY-1;}
        else if(MyA==7)
            {T_x=MyX;T_y=MyY-1;}
        else if(MyA==8)
            {T_x=MyX-1;T_y=MyY-1;}
        }
        if(CheckCrass(T_x,T_y)>2&&CrassM==false){
            TIM_Cmd(TIM2,DISABLE);//关闭步进用定时器
            CrassM=true;//写入交叉路口检测标志
            ImpactCom(T_x,T_y);//调用防碰撞通讯函数
            }
        else if(CheckCrass(T_x,T_y)>2&&CrassM==true){}
        else if(CheckCrass(T_x,T_y)<=2&&crassm==true){             crassm="false;//清除交叉路口检测标志"         } } 机器人路沿计算函数 输入参数：l左侧距，r右侧距 输出参数：左比总 double getecc(double l,double r){     return l (r+l); 路径校准函数 输入参数：无 输出参数：修正值 左轮 右轮 未完成 checkway(void){ return fix value  need each 1s used it          mya;     double r; l;     r="0;"     l="0;"     if(mya="=1){"         do{             l++;         }while(checkmap(7)="=true);"             r++;         }while(checkmap(3)="=true);"     }     else if(mya="=2){"         }while(checkmap(8)="=true);"         }while(checkmap(4)="=true);"         }while(checkmap(1)="=true);"         }while(checkmap(5)="=true);"         }while(checkmap(2)="=true);"         }while(checkmap(6)="=true);"            pv="getEcc(1,1);//其中1要改为左右超声测距值"     le="E;"     is="IS+E;"     e="(l/(l+r))-Pv;"     ra="E-LE;"   cv="E*Kp+RA*Kd+Ki*IS;" (cv*(l+r)); 标准转弯函数 输入参数：tangle（目标角度) 输出参数：无 void turnto(double tangle){     moto_list new;     if(tangle>0){
        New.DirectionLeft=1;
        New.DirectionRight=0;
    }
    else if(Tangle<0){         new.directionleft="0;"         new.directionright="1;"     }     new.timeleft="(long)(sqrt(Wheelbase)*PI*(Tangle/180))/(Wheellength*(0.9/32));"     new.timeright="(long)(sqrt(Wheelbase)*PI*(Tangle/180))/(Wheellength*(0.9/32));"     trunm="true;"     motosetting(&new);     while(finishm!="true);"     finishm="false;" } 前往点函数 输入参数：new 位置指针 输出参数：无 important void movecoo(loadinglist* new){     loadinglist diff;     recursion_cheak(new); int recursion_cheak(loadinglist*     creatmodel(1,new);     int i;     double s="2;" c="0;" crassn[2]="{0,0};"     while(1){      for(i="0;i<=7;i++){"          if(model<i>[1]==S){
            C++;
            CrassN[C-1]=i;
         }
     }
     if(C<=0){          if(c="=1){"              if(checkmap(i)="=true)break;" } else if(new->X==MyX||New->Y==MyY||(New->X==MyX&&New->Y==MyY)){
                    
             }
             else{
                 Recursion_Cheak(New);
             }
                
         }
     }
     else{
         S-=0.5;
     }
    }
    return i;
}
//充电校准函数
//输入参数：无
//输出参数：无
void ChargeMove(void){
       
}
   
void CreatModel(int N,LoadingList* New){
    int j;
    if(New->MapX==MapNumber(1)&&New->MapY ==MapNumber(2)){
        if(New->X>MyX){
            if(New->Y>MyY){
            for(j=0;j<=7;j++){             model[j][n]="ComModel_4[j];"             }             if(new->Y==MyY){
            for(j=0;j<=7;j++){             model[j][n]="ComModel_3[j];  "             }             if(new->Y<myy){             for(j="0;j<=7;j++){"             model[j][n]="ComModel_2[j];  "             }         }         if(new->X==MyX){
            if(New->Y>MyY){
            for(j=0;j<=7;j++){             model[j][n]="ComModel_5[j];  "             }             if(new->Y<myy){             for(j="0;j<=7;j++){"             model[j][n]="ComModel_1[j];  "             }         }         if(new->X<myx){             if(new->Y>MyY){
            for(j=0;j<=7;j++){             model[j][n]="ComModel_6[j];  "             }             if(new->Y==MyY){
            for(j=0;j<=7;j++){             model[j][n]="ComModel_7[j];  "             }             if(new->Y<myy){             for(j="0;j<=7;j++){"             model[j][n]="ComModel_8[j];  "             }         }     }     else{         if(new->MapX>MapNumber(1)){
            if(New->MapY>MapNumber(2)){
            for(j=0;j<=7;j++){             model[j][n]="ComModel_4[j];  "             }             if(new->MapY==MapNumber(2)){
            for(j=0;j<=7;j++){             model[j][n]="ComModel_3[j];  "             }             if(new->MapY<mapnumber(2)){             for(j="0;j<=7;j++){"             model[j][n]="ComModel_2[j];  "             }         }         if(new->MapX==MapNumber(1)){
            if(New->MapY>MapNumber(2)){
            for(j=0;j<=7;j++){             model[j][n]="ComModel_5[j];  "             }             if(new->MapY<mapnumber(2)){             for(j="0;j<=7;j++){"             model[j][n]="ComModel_1[j];  "             }         }         if(new->MapX<mapnumber(1)){             if(new->MapY>MapNumber(2)){
            for(j=0;j<=7;j++){             model[j][n]="ComModel_6[j];  "             }             if(new->MapY==MapNumber(2)){
            for(j=0;j<=7;j++){             model[j][n]="ComModel_7[j];  "             }             if(new->MapY<mapnumber(2)){             for(j="0;j<=7;j++){"             model[j][n]="ComModel_8[j];  "             }         }     } }< i></mapnumber(2)){></=7;j++){></=7;j++){></mapnumber(1)){></mapnumber(2)){></=7;j++){></mapnumber(2)){></=7;j++){></=7;j++){></myy){></=7;j++){></=7;j++){></myx){></myy){></=7;j++){></myy){></=7;j++){></=7;j++){></=0){></0){></=2&&crassm==true){></i></code>

<i><code class="lang-cpp"><i>#include "Map_all.h"
#include "MathMap.h"
struct LiLa{
    double LA;
    double LI;
};
struct Black{
    struct LiLa My;
    int MoveType[8];
};
struct Coo{
    int X;
    int Y;
};
struct Black MAP[300][300];//X,Y
int List[300];
struct Coo MapNumber;
int MyX;
int MyY;
int MyA;
bool StopM;
int l;
//移动模式：
//  8  1  2
//  7     3
//  6  5  4
//  1为正北
//地图移动模式
//    1
//  4   2
//    3
//地图形式
//  (0,0)(0,1)(0,2)(0,3)……
//  (1,0)(1,1)(1,2)(1,2)……
//  (2,0)(2,1)(2,2)(2,3)……
//  ……
   
//地图加载函数
//输入参数：T移动模式
//输出参数：无
void loadmap(int T){// 1:up,,2:down,3:left,;4;right,0:beginmap                no complete
long i;
long j;
    switch(T){
        case 1:{
        LoadingList New;
        MapReport PMap;
        MapNumber.X=MapNumber.X;
        MapNumber.Y=MapNumber.Y-1;
        New.MapX=MapNumber.X;
        New.MapY=MapNumber.Y;
        for(i=0;i<=300;i++){             for(j="0;j<=300;j++){"             new.x="i;"             new.y="j;"             pmap="SDGetMap(&New);"             map<i>[j].My.LA=PMap.La;
            MAP<i>[j].My.LI=PMap.Li;
            for(l=0;l<=8;l++){                 map<i>[j].MoveType[l]=PMap.MoveT[l];
                }
            }
        }
            break;
        }
        case 2:{
        LoadingList New;
        MapReport PMap;
        MapNumber.X=MapNumber.X+1;
        MapNumber.Y=MapNumber.Y;
        New.MapX=MapNumber.X;
        New.MapY=MapNumber.Y;
        for(i=0;i<=300;i++){             for(j="0;j<=300;j++){"             new.x="i;"             new.y="j;"             pmap="SDGetMap(&New);"             map<i>[j].My.LA=PMap.La;
            MAP<i>[j].My.LI=PMap.Li;
            for(l=0;l<=8;l++){                 map<i>[j].MoveType[l]=PMap.MoveT[l];
                }
            }
        }
            break;
        }
        case 3:{
        LoadingList New;
        MapReport PMap;
        MapNumber.X=MapNumber.X;
        MapNumber.Y=MapNumber.Y+1;
        New.MapX=MapNumber.X;
        New.MapY=MapNumber.Y;
        for(i=0;i<=300;i++){             for(j="0;j<=300;j++){"             new.x="i;"             new.y="j;"             pmap="SDGetMap(&New);"             map<i>[j].My.LA=PMap.La;
            MAP<i>[j].My.LI=PMap.Li;
            for(l=0;l<=8;l++){                 map<i>[j].MoveType[l]=PMap.MoveT[l];
                }
            }
        }
            break;
        }
        case 4:{
        LoadingList New;
        MapReport PMap;
        MapNumber.X=MapNumber.X-1;
        MapNumber.Y=MapNumber.Y;
        New.MapX=MapNumber.X;
        New.MapY=MapNumber.Y;
        for(i=0;i<=300;i++){             for(j="0;j<=300;j++){"             new.x="i;"             new.y="j;"             pmap="SDGetMap(&New);"             map<i>[j].My.LA=PMap.La;
            MAP<i>[j].My.LI=PMap.Li;
            for(l=0;l<=8;l++){                 map<i>[j].MoveType[l]=PMap.MoveT[l];
                }
            }
        }
            break;
        }
        case 0:{
        LoadingList New;
        MapReport PMap;
        MyX=0;
        MyY=0;
        MapNumber.X=0;
        MapNumber.Y=0;
        New.MapX=MapNumber.X;
        New.MapY=MapNumber.Y;
        for(i=0;i<=300;i++){             for(j="0;j<=300;j++){"             new.x="i;"             new.y="j;"             pmap="SDGetMap(&New);"             map<i>[j].My.LA=PMap.La;
            MAP<i>[j].My.LI=PMap.Li;
            for(l=0;l<=8;l++){                 map<i>[j].MoveType[l]=PMap.MoveT[l];
                }
            }
        }
            break;
        }
    }
}
//主移动函数函数
//输入参数：T移动模式，1：预定路线，2：随机路线，3：控制室控制
//输出参数：无
void MainMove(int T){
    loadmap(0);
    l=0;//error
    while(T==1){
     int i;
        if(List[l]==List[l-1]){
            while(List[l]==List[l+i]){i++;}
                MoveTo(i);
                //brack while
                l=l+i-1;
                if(StopM==true){break;}
        }
        else{
            TurnTo(45*(List[l]-List[l-1]));
            MoveTo(1);
            if(StopM==true){break;}
        }
        if(l<=300){             break;         }         else{             l++;     }     while(t="=2){"         int r; lastr;         do{         r="radom(9,1,2);//black" ad io         }while(checkmap(r)="=true);  "         if(r="=LastR){"                 moveto(1);             turnto(45*(r-lastr));             moveto(1);             if(stopm="=true){break;}"         lastr="R;"      *     command from computer     *     if(stopm="=true){break;}"                   } 地图检测函数 输入参数：type移动模式 输出参数：bool true表示可行 bool checkmap(int type){ error         if(map[myx][myy].movetype[type]="=1){"             return true; false; 检测交叉路口函数 输入参数：x,y坐标 输出参数：通道数 int checkcrass(int x,int y){     int i; out="0;"             for(i="0;I<=8;I++){"                 if(map[x][y].movetype<i>==true){
                    Out++;
                    }
                }
            return Out;
}
void MapChan(int X,int Y){
    MapNumber.X=MapNumber.X+X;
    MapNumber.Y=MapNumber.X+Y;
}
int MapNumberC(int C){
    if(C==1){
        return MapNumber.X;
    }
    else if(C==2){
        return MapNumber.Y;
    }
}</=300){></=8;l++){></i></=300;i++){></=8;l++){></i></=300;i++){></=8;l++){></i></=300;i++){></=8;l++){></i></=300;i++){></=8;l++){></i></=300;i++){></i></i></i></i></i></i></i></i></i></i></i></i></code></i>