不知道为啥用不了代码窗，所有只能发文本上来。有一些底层代码还没有完成，如果有人愿意提意见的话，我会吧后续的也贴上来
               正文
这个移动程序是以一个结构体二维数组作为地图，以步进电机为移动机构。有5个文件。
Move.c

#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;
//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;
int i;
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);
}
//路径校准函数
//输入参数：无
//输出参数：修正值 左轮/右轮
//未完成
double CheckWay(void){//return fix value  need each 1s used it    
//MyA;
double r;
double l;
r=0;
l=0;
if(MyA==1){
do{
l++;
}while(CheckMap(7)==true);
do{
r++;
}while(CheckMap(3)==true);
}
else if(MyA==2){
do{
l++;
}while(CheckMap(8)==true);
do{
r++;
}while(CheckMap(4)==true);
}
else if(MyA==3){
do{
l++;
}while(CheckMap(1)==true);
do{
r++;
}while(CheckMap(5)==true);
}
else if(MyA==4){
do{
l++;
}while(CheckMap(2)==true);
do{
r++;
}while(CheckMap(6)==true);
}
else if(MyA==5){
do{
r++;
}while(CheckMap(7)==true);
do{
l++;
}while(CheckMap(3)==true);
}
else if(MyA==6){
do{
r++;
}while(CheckMap(8)==true);
do{
l++;
}while(CheckMap(4)==true);
}
else if(MyA==7){
do{
r++;
}while(CheckMap(1)==true);
do{
l++;
}while(CheckMap(5)==true);
}
else if(MyA==8){
do{
r++;
}while(CheckMap(2)==true);
do{
l++;
}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;
return (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;
}
Map.c

#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,Yint 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 completelong i;
long j; switch(T){
case 1:{ LoadingList New;
MapReport PMap; MapNumber.X=MapNumber.X;
MapNumber.Y=MapNumber.Y-1; XXXXXXpX=MapNumber.X;
XXXXXXpY=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].XXXXX=XXXXXXX; MAP[i][j].XXXXX=XXXXXXX;
for(l=0;l<=8;l++){ MAP[i][j].MoveType[l]=XXXXXXXveT[l];
} }
} break;
} case 2:{
LoadingList New; MapReport PMap;
MapNumber.X=MapNumber.X+1; MapNumber.Y=MapNumber.Y;
XXXXXXpX=MapNumber.X; XXXXXXpY=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].XXXXX=XXXXXXX;
MAP[i][j].XXXXX=XXXXXXX; for(l=0;l<=8;l++){
MAP[i][j].MoveType[l]=XXXXXXXveT[l]; }
} }
break; }
case 3:{ LoadingList New;
MapReport PMap; MapNumber.X=MapNumber.X;
MapNumber.Y=MapNumber.Y+1; XXXXXXpX=MapNumber.X;
XXXXXXpY=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].XXXXX=XXXXXXX; MAP[i][j].XXXXX=XXXXXXX;
for(l=0;l<=8;l++){ MAP[i][j].MoveType[l]=XXXXXXXveT[l];
} }
} break;
} case 4:{
LoadingList New; MapReport PMap;
MapNumber.X=MapNumber.X-1; MapNumber.Y=MapNumber.Y;
XXXXXXpX=MapNumber.X; XXXXXXpY=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].XXXXX=XXXXXXX;
MAP[i][j].XXXXX=XXXXXXX; for(l=0;l<=8;l++){
MAP[i][j].MoveType[l]=XXXXXXXveT[l]; }
} }
break; }
case 0:{ LoadingList New;
MapReport PMap; MyX=0;
MyY=0; MapNumber.X=0;
MapNumber.Y=0; XXXXXXpX=MapNumber.X;
XXXXXXpY=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].XXXXX=XXXXXXX; MAP[i][j].XXXXX=XXXXXXX;
for(l=0;l<=8;l++){ MAP[i][j].MoveType[l]=XXXXXXXveT[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]));
if(StopM==true){break;} }
if(l<=300){ break;
} else{
l++; }
} while(T==2){
int R; int LastR;
do{ R=radom(9,1,2);//black AD IO
}while(CheckMap(R)==true);  if(R==LastR){
MoveTo(1); }
else{ TurnTo(45*(R-LastR));
if(StopM==true){break;} }
LastR=R; }
while(T==3){ /*
command from computer */
if(StopM==true){break;} }
while(T==4){//debug
}}
//地图检测函数//输入参数：type移动模式
//输出参数：bool True表示可行bool CheckMap(int type){//error
if(MAP[MyX][MyY].MoveType[type]==1){ return true;
} else{
return false; }
}//检测交叉路口函数
//输入参数：X,Y坐标//输出参数：通道数
int CheckCrass(int X,int Y){ int I;
int 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;}
STM32Jack.c
#include "Map_all.h"#define P1 GPIO_Pin_1
#define P2 GPIO_Pin_2#define P3 GPIO_Pin_3
#define P4 GPIO_Pin_4#define P5 GPIO_Pin_5
#define P6 GPIO_Pin_6#define P7 GPIO_Pin_7
#define P8 GPIO_Pin_8#define P9 GPIO_Pin_9
#define P10 GPIO_Pin_10#define P11 GPIO_Pin_11
#define P12 GPIO_Pin_12#define P13 GPIO_Pin_13
#define P14 GPIO_Pin_14#define P15 GPIO_Pin_15
#define Pall GPIO_Pin_ALL#define A GPIOA
#define B GPIOB#define C GPIOC
#define D GPIOD#define E GPIOE
#define a RCC_APB2Periph_GPIOA#define b RCC_APB2Periph_GPIOB
#define c RCC_APB2Periph_GPIOC#define d RCC_APB2Periph_GPIOD
#define e RCC_APB2Periph_GPIOE#define RTIM2 RCC_APB1Periph_TIM2
#define RTIM3 RCC_APB1Periph_TIM3#define RTIM4 RCC_APB1Periph_TIM4
#define bool int#define true 1
#define false 0bool FinishM;
bool FinishL,FinishR;long TimeR;
long TimeTL;long TimeTR;
double length;extern bool TrunM;
extern int MyX;extern int MyY;
extern int MyA;double MoveHistry[50];
long HyStage=0;//移动模式：
//  8  1  2//  7     3
//  6  5  4//  1为正北
//int a;//a=ReadIO(A,P1);
bool ReadIO(GPIO_TypeDef* MainType,uint16_t SubType){ return GPIO_ReadInputDataBit(MainType,SubType);
}void GPIOInit(GPIO_TypeDef* MainType1,uint32_t MainType2,uint16_t SubType){//
GPIO_InitTypeDef  GPIO_InitStructure; RCC_APB2PeriphClockCmd(MainType2,ENABLE);
GPIO_XXXXXXXXXXXXXXXXIO_Pin = SubType; GPIO_XXXXXXXXXXXXXXXXIO_Mode = GPIO_Mode_Out_PP;
GPIO_XXXXXXXXXXXXXXXXIO_Speed = GPIO_Speed_50MHz; GPIO_Init(MainType1, &GPIO_InitStructure);
}void WriteIO(GPIO_TypeDef* MainType,uint16_t SubType,bool bit){
switch(bit){ case 1:
{ GPIO_SetBits(MainType,SubType);
break;} case 0:
{ GPIO_ResetBits(MainType,SubType);
break;} }
}/************************************************************************************/
void NVICInit(uint8_t Chan,uint8_t CPP,uint8_t SP,FunctionalState Cmd){ NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = Chan;  //TIM3中断 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = CPP;  //先占优先级0级
NVIC_InitStructure.NVIC_IRQChannelSubPriority = SP;  //从优先级3级 NVIC_InitStructure.NVIC_IRQChannelCmd = Cmd; //IRQ通道被使能
NVIC_Init(&NVIC_InitStructure);  //初始化NVIC寄存器 SystemInit();
}void N_TIMxInit(TIM_TypeDef* TIMx,uint32_t RTIMx,uint16_t arr,uint16_t psc){
TIM_TimeBaseInitTypeDef TIM_SETTING; RCC_APB1PeriphClockCmd(RTIMx, ENABLE);
if(TIMx==TIM2){ NVICInit(TIM2_IRQn,0,3,ENABLE);
} else if(TIMx==TIM3){
NVICInit(TIM3_IRQn,0,3,ENABLE); }
else if(TIMx==TIM4){ NVICInit(TIM4_IRQn,0,3,ENABLE);
} TIM_SETTING.TIM_Period = arr;
TIM_SETTING.TIM_Prescaler =psc;  TIM_SETTING.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_SETTING.TIM_CounterMode = TIM_CounterMode_Up;   TIM_TimeBaseInit(TIMx, &TIM_SETTING);
}void MotoInit(){
N_TIMxInit(TIM2,RTIM2,1,7199);   //0.1ms 10KHz}
void TIM3_IRQHandler(void){ if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET)
{ TIM_ClearITPendingBit(TIM2, TIM_IT_Update  );  
if(FinishM!=true){ if(TimeR>=TimeTL){
FinishL=true; /*
GPIO TO MOTOR */
} if(TimeR>=TimeTR){
FinishR=true; /*
GPIO TO MOTOR */
} if(FinishR&&FinishL)
{ if(TrunM!=true){
length=TimeR*(0.9/32)*Wheellength; CooMove();
UpdataToHy(); }
else{ TrunM=false;
} FinishR=false;
FinishL=false; FinishM=true;
TimeR=0; WriteIO(A,P5,false);
TIM_Cmd(TIM2,DISABLE); }
else{ TimeR++;
} }
}}

//NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
void MotoSetting(Moto_List* MotoOrder){ //A.1:cw L    A.2:cw R    A.3:clk L    A.4;clk R    A.5:EN if(MotoOrder->DirectionLeft==1){
if(MotoOrder->DirectionRight==1){ WriteIO(A,P1,true);//L
WriteIO(A,P2,true);//R }
else if(MotoOrder->DirectionRight==0){ WriteIO(A,P1,true);
WriteIO(A,P2,false); }
} else if(MotoOrder->DirectionLeft==0){
if(MotoOrder->DirectionRight==1){ WriteIO(A,P1,false);
WriteIO(A,P2,true); }
else if(MotoOrder->DirectionRight==0){     WriteIO(A,P1,false);
WriteIO(A,P2,false); }
} TimeTL=MotoOrder->TimeLeft*2;
TimeTR=MotoOrder->TimeRight*2; FinishM=false;
WriteIO(A,P5,true); TIM_Cmd(TIM2,ENABLE);
}

//SD接口函数//输入参数：New 输入地图标号
//输出参数：MapReport 输出地图数据MapReport SDGetMap(LoadingList* New){
MapReport Return;
return Return;}
//移动历史上传函数//输入参数：无
//输出参数：无void UpdataToHy(){
if(HyStage>=50){ MoveHistry[HyStage-50]=length;
} else{
  MoveHistry[HyStage]=length; }
}//当前坐标计算函数
//输入参数：无//输出参数：无
void CooMove(){ if(MyA==1)
{MyX=MyX-(int)length;} else if(MyA==2)
{MyX=MyX-(int)(length/1.41421356237); MyY=MyY+(int)(length/1.41421356237);}
else if(MyA==3) {MyX=MyX;MyY=MyY+(int)length;}
else if(MyA==4) {MyX=MyX+(int)(length/1.41421356237);
MyY=MyY+(int)(length/1.41421356237);} else if(MyA==5)
{MyX=MyX+(int)length; MyY=MyY;}
else if(MyA==6) {MyX=MyX+(int)(length/1.41421356237);
MyY=MyY-(int)(length/1.41421356237);} else if(MyA==7)
{MyX=MyX;MyY=MyY-(int)length;} else if(MyA==8)
{MyX=MyX-(int)(length/1.41421356237); MyY=MyY-(int)(length/1.41421356237);}
//地图移动模式//    1
//  4   2//    3
if(MyX>300){ loadmap(2);
MyX=0; MapChan(1,0);
} else if(MyY>300){
loadmap(3); MyY=0;
MapChan(0,1); }
else if(MyX<0){ loadmap(4);
MyX=300; MapChan(-1,0);
} else if(MyY<0){
loadmap(1); MyY=300;
MapChan(0,-1); }
}//防碰撞通讯函数
//输入参数：无//输出参数：交叉路口坐标
//备注：1.发送本机序列号，2.发送交叉路口坐标，3.等待500ms，如果没有回应，启动步进void ImpactCom(int X,int Y){
}