电赛常用的程序有哪些？

一、电赛的故事
这是我的第三次电赛，大一炮灰，比较轻松，大二大三就很累，四天基本只能睡6个小时，所以今天从2点睡到了17点。由于今年的电赛比较诡异，没有纯粹的控制题，所以我这个做控制的比较被动，选择的电磁炮题。


二、准备程序
三次电赛准备了很多的程序，这里将一些常用的程序分享出来。


1、PID
控制题的必备品，谁用谁知道


头文件pid.h


#ifndef _PID_H
#define _PID_H

typedef struct _positional_pid{
    //PID的基本参数
    double GoalVale;      //目标值
    double ActualVale;    //真实值
    double Error;         //误差
    double LastError;     //上一次的误差
    double Kp,Ki,Kd;      //PID三参数
    double T;             //周期
    double Output;        //输出
    double Integral;      //积分值

    //状态参数
    int Flag_First;     //首次运算的标志
    double MaxOutput;  //输出限幅
    double MinOutput;  //输出限幅
    double IntePartValue;   //积分分离阈值
                double MAXInte;         //积分限幅
    double NoneActValue; //不动作区阈值
}PositionalPid;

typedef struct _incremental_pid{
    //PID的基本参数
    double GoalVale;      //目标值
    double ActualVale;    //真实值
    double Error;         //误差
    double LastError1;    //上一次的误差
    double LastError2;    //上上次的误差
    double Kp,Ki,Kd;      //PID三参数
    double T;             //周期
    double Output;        //输出

    //状态参数
    int Flag_First;     //首次运算的标志
    double MaxOutput;  //输出限幅
    double MinOutput;  //输出限幅
    double NoneActValue; //不动作区阈值
}IncrementalPid;

//位置式PID计算公式
void positionalPid_Cal(PositionalPid *pid , double ActualValue);

//增量式PID计算公式
void incrementalPid_Cal(IncrementalPid *pid , double ActualValue);

#endif
c文件


#include "pid.h"
#include

/*************************************************************************
*Name   : positionalPid_Cal
*Funs   : 计算位置式PID的输出
*Input  : pid，位置式PID的结构体 ; ActualValue ，控制对象的测量值
*Output : None(将计算的结构存到pid结构体的参数里面)
*************************************************************************/
void positionalPid_Cal(PositionalPid *pid , double ActualValue){
    pid->ActualVale = ActualValue;
    pid->Error = pid->GoalVale - pid->ActualVale; //设定值-目前值
    if(fabs(pid->Error) < pid->NoneActValue){  //死区
        pid->Integral=0;
        return ;
    }
    /*计算p*/
    double p_temp = pid->Kp * pid->Error;

    /*计算i,同时积分分离*/
    if(fabs(pid->Error) > pid->IntePartValue){
        pid->Integral=0;
    }else{
        if(fabs(pid->Integral)>pid->MAXInte)
            pid->Integral=pid->Integral>0?pid->MAXInte:(-pid->MAXInte);
        else
            pid->Integral += pid->Error;
    }
    double i_temp=pid->Integral * pid->Ki;
    double d_temp;
    /*首次不计算微分*/
    if(pid->Flag_First){
        d_temp=pid->Kd*(pid->Error-pid->LastError);
        pid->LastError=pid->Error;
    }else{
        pid->Flag_First=1;
        d_temp=0;
        pid->LastError=pid->Error;
    }
    pid->Output=p_temp+i_temp+d_temp;
    /*输出限幅*/
    if(pid->Output < pid->MinOutput)
        pid->Output = pid->MinOutput;
    if(pid->Output > pid->MaxOutput)
        pid->Output = pid->MaxOutput;
    pid->LastError = pid->Error;
}

/*******************************************************************************
*Name   : positionalPid_Cal
*Funs   : 计算位置式PID的输出
*Input  : pid，位置式PID的结构体 ; ActualValue ，控制对象的测量值
*Output : None(将计算的结构存到pid结构体的参数里面)
*******************************************************************************/
void incrementalPid_Cal(IncrementalPid *pid , double ActualValue){
    pid->ActualVale = ActualValue;
    pid->Error = pid->GoalVale - pid->ActualVale; //设定值-目前值
    if(fabs(pid->Error) < pid->NoneActValue){  //死区
        pid->Output=0;
        return ;
    }
    if(pid->Flag_First>=2){
        pid->Output = pid->Kp * pid->Error - pid->Ki*pid->LastError1 + pid->Kd *         pid->LastError2;
        /*输出限幅*/
        if(pid->Output < pid->MinOutput)
            pid->Output = pid->MinOutput;
        if(pid->Output > pid->MaxOutput)
            pid->Output = pid->MaxOutput;
    }else{
        pid->Flag_First++;
    }
    pid->LastError2 = pid->LastError1;
    pid->LastError1 = pid->Error;
}
里面有两种形式的PID，位置式和增量式，建议使用位置式的PID，因为两种本质是一样的，但是增量式PID调参不好调（个人认为），我的里面加入了PID的常见操作，包括死区设置、输出限幅、积分限幅、积分分离。使用的时候只需要将我的文件加进工程，初始化一个PID的结构体，然后调用相应的函数进行计算就可以了，我写的函数都是进行了实际测试的，基本没有BUG，大家可以放心使用。


2、滤波
头文件


#ifndef _MYFILTER_H
#define _MYFILTER_H
#define SIZE 5

typedef float Element;

typedef struct FILTER {
        Element Data[SIZE];  //滤波核
        int head;            //队头
        int tail;            //队尾
        int flag;                         //用来判断是否已经有足够的数据
        int Mode;                         //表示滤波的方法
} Filter;

typedef struct KALAMFILTER {
        double Q;
        double R;
        double P_Last;
        double X_Last, Kg, X_mid, P_Mid, X_Now, P_Now;
        int flag;
}KalManFilter;

//初始化滤波器
void initFilter(Filter* f ,int Mode);

//初始化卡尔曼滤波器
void initKalManFilter(KalManFilter* f,double Q,double R,double P_Last);

//平均滤波
Element AverageFilter(Filter* f,Element);

//中值滤波
Element MiddleFilter(Filter* f, Element);

//卡尔曼滤波
Element KalManFilterCal(KalManFilter *,Element);
#endif
c文件


#include "myfilter.h"
#include

/******************************************************************
*Name        :initFliter
*Funs        :初始化滤波器
*Input        :f表示待初始化的滤波器，size表示滤波核的大小，Mode表示滤波方法
*Output :None
*********************************************************************/
void initFilter(Filter* f,int Mode) {
    f->flag = 0;
    f->Mode = Mode;
    f->head = 0;
    f->tail = 0;
}

/*********************************************************************
*Name        :initKalManFilter
*Funs        :初始化卡尔曼滤波器
*Input        :f表示待初始化的滤波器，Q表示系统噪声，R表示测量噪声，P_Last表示系统状态协方差初始值
*Output :None
**********************************************************************/
void initKalManFilter(KalManFilter* f, double Q, double R, double P_Last) {
    f->P_Last = P_Last;
    f->Q = Q;
    f->R = R;
    f->flag = 0;
}

/*******************************************************************
*Name        :AverageFilter
*Funs        :滑动平均滤波
*Input        :f表示滤波器，
*Output :None
*******************************************************************/
Element AverageFilter(Filter* f, Element data) {
    Element res=0;
    if (f->flag < SIZE) {
        f->Data[f->tail++] = data;
        f->flag++;
        for (int i = 0; i < f->flag; i++) {
            res += f->Data;
        }
        return res / f->flag;
    }
    else {
        f->head++;     //出队
        f->head = f->head%SIZE;
        f->tail++;
        f->tail = f->tail%SIZE;
        f->Data[f->tail] = data;
        for (int i = 0; i < SIZE; i++) {
                res += f->Data;
        }
        return res / SIZE;
    }
}

/**************************************************************
*Name        :MiddleFilter
*Funs        :滑动中值滤波
*Input        :f表示滤波器，data为滤波器的输入数据
*Output :None
***************************************************************/
Element MiddleFilter(Filter* f, Element data) {
    Element res = 0;
    if (f->flag         f->Data[f->tail++] = data;
        f->flag++;
        for (int i = 0; i < f->flag; i++) {
                res += f->Data;
        }
        return res / f->flag;
    }
    else {
        f->head++;     //出队
        f->head = f->head%SIZE;
        f->tail++;
        f->tail = f->tail%SIZE;
        f->Data[f->tail] = data;
        //取中值
        Element temp[SIZE];
        for (int i = 0; i < SIZE; i++)
            temp = f->Data;
            Element p;
            for (int i = 0; i <= SIZE/2; i++) {
                for (int j = 0; j < SIZE-i-1; j++) {
                    if (temp[j] > temp[j + 1]) {
                        p = temp[j];
                        temp[j] = temp[j + 1];
                        temp[j + 1] = p;
                    }
                    }
        }
        return temp[SIZE/2];
    }
}

/***********************************************************
*Name        :KalManFilterCal
*Funs        :卡尔曼滤波
*Input        :data为滤波器的输入数据
*Output :None
***********************************************************/
Element KalManFilterCal(KalManFilter *f, Element Data){
    if (!f->flag) {
        f->flag = 1;
        f->X_Last = Data;
    }
    f->X_mid = f->X_Last;
    f->P_Mid = f->P_Last + f->Q;
    f->Kg = f->P_Mid / (f->P_Mid+f->R);
    f->X_Now = f->X_mid + f->Kg*(Data - f->X_mid);
    f->P_Now = (1 - f->Kg)*f->P_Mid;
    return f->X_Now;
}
包含三个滤波器，均值滤波、中值滤波、卡尔曼滤波。其中均值滤波和中值滤波已经经过我的验证了，卡尔曼滤波并没有经过我的验证。使用的时候，只需要初始化一下滤波器，然后调用相应的函数即可。


3、菜单
比赛的时候，一般需要一个界面，用来人机交互，我的这个菜单可以实现任意级的菜单，相当于一个只有执行功能的文件系统


头文件


#ifndef _QZQMENU_H
#define _QZQMENU_H
#include "sys.h"
#define LCD_WIDTH  240
#define LCD_HEIGHT 320
typedef struct Menu_Item{
        u8  ChildCount;                   //子菜单个数      
        u8  CurrentChild;                 //当前孩子个数
        u8 Mode;                          //菜单类型
        char * name;                      //菜单名字
        void (*Action)();                 //菜单被选中时需执行的函数指针   
        struct Menu_Item **ChildrenMenus; //子菜单结构体数组
        struct Menu_Item *ParentMenus;   //父菜单
}MenuItem;

//创建一个菜单子项
int createOneItem(MenuItem *item,u8 mode,char* name,void (*fun)(),int ChildNum);

//建立父亲关系
int Parent(MenuItem * A,MenuItem *B);

//父亲专用行为，打印所有的子项到屏幕上
void ParentAction(MenuItem *item);

//开始行动
void StartMenu(MenuItem *ALL);

#endif
c文件


#include "qzqmenu.h"
#include "lcd.h"
#include
#include
#include
#include "My_Key.h"
#include "delay.h"
/***********************************************************************************
*Name   : createOneItem
*Fun    : 创建一个菜单子项，并初始化其参数
*Input  : item，菜单子项指针；fun，菜单功能；ChildNum，该子项的子菜单个数；ParentNum 该子项的父菜单个数
          mode 表示模式，0表示功能项，1表示集合项
*Output : 0 表示初始化失败；1表示初始化成功
************************************************************************************/
int createOneItem(MenuItem *item,u8 mode,char* name,void (*fun)(),int ChildNum){
    item->ChildrenMenus = (MenuItem**)malloc(ChildNum*sizeof(MenuItem*));
    item->ChildCount=ChildNum;
    item->Action=fun;
    item->CurrentChild=0;
    item->name = name;
    item->Mode =mode;
    item->ParentMenus=NULL;
    //检查是否成功申请内存
    if(item->ChildrenMenus==0&&ChildNum){
            return 0;
    }else
        return 1;
}


/***********************************************************************
*Name   : Parent
*Fun    : 绑定两个菜单的父子关系
*Input  : A，父亲；B 儿子
*Output : 0 表示绑定失败 ； 1 表示绑定成功
***********************************************************************/
int Parent(MenuItem * A,MenuItem *B){
    //检查A的孩子是否已经满了
    if(A->CurrentChild>=A->ChildCount)
            return 0;
    if(A->Mode==0)
            return 0;
    A->ChildrenMenus[A->CurrentChild++] = B;
    B->ParentMenus= A;
    return 1;
}

//父亲专用行为
void ParentAction(MenuItem *item){
    LCD_Clear(WHITE);
    int XBais=20,YBais=40;
    int YAdd =30;
    char buf[50];
    POINT_COLOR=BLUE;
    LCD_ShowString(0,5,200,30,16,item->name);
    for(int i=0;iCurrentChild;i++){
            sprintf(buf,"%d  %s",i+1,item->ChildrenMenus->name);
            POINT_COLOR=RED;
            LCD_DrawRectangle(XBais-5,YBais+i*YAdd-5,XBais+200,YBais+i*YAdd+25);
            if(item->ChildrenMenus->Mode)
                    POINT_COLOR=RED;
            else
                POINT_COLOR=BLACK;
            LCD_ShowString(XBais,YBais+i*YAdd,200,30,16,buf);
    }
}


/******************************************************************
*Name   : StartMenu
*Fun    : 绑定两个菜单的父子关系
*Input  : ALL 表示顶层子项
*Output : None
*****************************************************************/
void StartMenu(MenuItem *ALL){
    if(!ALL->Mode)
        return;
        u8 key;
        MenuItem *cur = ALL;
        ParentAction(cur);
        while(1){
            while(!(key=My_Key_scan()))
                ;
                delay_ms(200);
            if(key==16){//返回上一级菜单
                if(cur->ParentMenus){
                        cur = cur->ParentMenus;
                        ParentAction(cur);
                }
            }else if(key>0&&key<=cur->CurrentChild){
                if(cur->ChildrenMenus[key-1]->Mode){
                    cur = cur->ChildrenMenus[key-1];
                    ParentAction(cur);
                }else{
                    if(cur->ChildrenMenus[key-1]->Action){
                        cur->ChildrenMenus[key-1]->Action();
                    }
                }
            }
        }
}
使用例子


void MenuInit(void){
    MenuItem Start,item1,item2,item3;
    MenuItem t1,t2,t3,t4,t5,t6,t;
       
    createOneItem(&t,1,"2019 NCSEDC",LED_1,6);
    createOneItem(&t1,1,"Basic Topic One",LED_1,0);
    createOneItem(&t2,1,"Basic Topic Two",LED_2,0);
    createOneItem(&t3,1,"Basic Topic Three",LED_3,0);
    createOneItem(&t4,1,"Extended Topic One",LED_4,0);
    createOneItem(&t5,1,"Extended Topic Tow",LED_5,0);
    createOneItem(&t6,1,"Others",LED_6,0);
       
    Parent(&t,&t1);
    Parent(&t,&t2);
    Parent(&t,&t3);
    Parent(&t,&t4);
    Parent(&t,&t5);
    Parent(&t,&t6);
    StartMenu(&t);
}
注：菜单需要配合按键和LCD来联合使用，这里按键我使用的是4x4按键，LCD是使用的正点原子的TFT配套例程，完整的工程会在后面给出链接


4、直流编码电机控速和控距
这份代码是同时控制三个电机的，因为当时正在做这个分为底层的电机驱动和中层的速度距离控制，中层里面提供了用户来设定速度和距离的接口。


1、底层头文件


#ifndef MYPWM_H
#define MYPWM_H
#include "sys.h"

#define FDIV (84)
#define FULL (500)

#define MOTOR1_FORWARD {PFout(0)=1;PFout(1)=0;}
#define MOTOR1_REVERSE {PFout(1)=1;PFout(0)=0;}
#define MOTOR2_FORWARD {PFout(2)=1;PFout(3)=0;}
#define MOTOR2_REVERSE {PFout(3)=1;PFout(2)=0;}
#define MOTOR3_FORWARD {PFout(4)=1;PFout(5)=0;}
#define MOTOR3_REVERSE {PFout(5)=1;PFout(4)=0;}
//电机1初始化，包括定时器、方向引脚等
void Motor1Init(void);

//设置占空比
void SetRate(float rate,u8 );

#endif
底层c文件


#include "motor.h"
#include "sys.h"
#include "usart.h"
#include "pid.h"

/******************************************************************
* Name   : setMotor1DirGpio
* Funs   : 初始化三个电机控制，包括定时器、方向引脚
* Input  : None
* Output : None
******************************************************************/
void static setMotor1DirGpio(void){
  GPIO_InitTypeDef  GPIO_InitStructure;
  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE); //使能GPIOF时钟

  //GPIOF0-5初始化设置
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1|GPIO_Pin_2 |             GPIO_Pin_3|GPIO_Pin_4 | GPIO_Pin_5;//0-5作为方向控制
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;         //普通输出模式
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;        //推挽输出
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;    //100MHz
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;          //上拉
  GPIO_Init(GPIOF, &GPIO_InitStructure);                //初始化GPIO
  MOTOR1_FORWARD;
  MOTOR2_FORWARD;
  MOTOR3_FORWARD;
}

void Motor1Init(void){
    GPIO_InitTypeDef GPIO_InitStructure;
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    TIM_OCInitTypeDef  TIM_OCInitStructure;
       
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);          //TIM3时钟使能   
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);         //使能PORTF时钟       
       
    GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM3); //GPIOC6复用为定时器3
    GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM3); //GPIOC7复用为定时器3
    GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM3); //GPIOC8复用为定时器3
       
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8; //GPIOC678第一、二、三通道做pwm波输出
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;        //复用功能
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;        //速度100MHz
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      //推挽复用输出
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        //上拉
    GPIO_Init(GPIOC,&GPIO_InitStructure);              //初始化PC678
       
    TIM_TimeBaseStructure.TIM_Prescaler=FDIV-1;  //定时器分频
    TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    TIM_TimeBaseStructure.TIM_Period=FULL-1;   //自动重装载值
    TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    TIM_TimeBaseInit(TIM3,&TIM_TimeBaseStructure);//初始化定时器3
       
    //初始化TIM3 Channel1 PWM模式         
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低
    TIM_OC1Init(TIM3, &TIM_OCInitStructure);  //根据T指定的参数初始化外设TIM3 OC1
    TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);  //使能TIM3在CCR1上的预装载寄存器       
       
    //初始化TIM3 Channel2 PWM模式         
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性
    TIM_OC2Init(TIM3, &TIM_OCInitStructure);  //根据T指定的参数初始化外设TIM3 OC2
    TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);  //使能TIM3在CCR2上的预装载寄存器       
       
    //初始化TIM3 Channel3 PWM模式         
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低
    TIM_OC3Init(TIM3, &TIM_OCInitStructure);  //根据T指定的参数初始化外设TIM3 OC3
    TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);  //使能TIM3在CCR3上的预装载寄存器       
       
    TIM_ARRPreloadConfig(TIM3,ENABLE);//ARPE使能，对预装载值做出更改后，立即生效
    TIM_Cmd(TIM3, ENABLE);  //使能TIM3       
    setMotor1DirGpio();
}  




/******************************************************
*函数名：SetRate
*输入：rate 占空比 取值-1到1;channel 表示哪一个通道，即哪一个电机，取值1-3
*输出：无
*功能：修改电机1-3的PWM波
*******************************************************/
void SetRate(float rate,u8 channel){
    if(rate>1)
        rate=1;
    if(rate<-1)
        rate=-1;
    if(rate<0){
        rate=-rate;
        if(channel==1){
                MOTOR1_REVERSE;
        }else if(channel==2){
                MOTOR2_REVERSE;
        }else if(channel==3){
            MOTOR3_REVERSE;
        }
    }else{
        if(channel==1){
            MOTOR1_FORWARD;
        }else if(channel==2){
            MOTOR2_FORWARD;
        }else if(channel==3){
            MOTOR3_FORWARD;
        }
    }
    if(channel==1){
        TIM_SetCompare1(TIM3,rate*FULL);
    }else if(channel==2){
        TIM_SetCompare2(TIM3,rate*FULL);
    }else if(channel==3){
        TIM_SetCompare3(TIM3,rate*FULL);
    }
}



3、中层头文件


#ifndef _CONTROL_H
#define _CONTROL_H
#include "sys.h"
#include "pid.h"
#define MINRATE 0.2
#define STATETHRE 30


typedef struct MOTORPARM{//此结构作为外部控制的接口
        int speed;                //电机当前速度
        int dis;                  //电机当前目标距离
        u8 flag_finish;           //动作完成标志
        u8 flag_state;            //调速或调距
        u8 number;                //电机编号
        float rate ;              //电机占空比
        IncrementalPid pid_speed; //电机运算pid
        PositionalPid pid_speed_p;//电机位置式速度pid
        PositionalPid pid_dis;    //电机位置pid
}MotorParm;

//变量声明
extern MotorParm motor1,motor2,motor3;

//控制电机
void MotorControl(float GoalSpeed);

//改变电机的速度
void setMotorSpeed(float speed);

//调速
void motorSpeed(MotorParm *motor);

//调距
void motorDistance(MotorParm *motor);

//电机状态更新
void motorStateUpdata(MotorParm *motor);

//电机过程控制
void motorProcessControl(MotorParm *car);

//控制初始化
void MotorsControlInit(void);

//更改电机1的目标
void setMotor1Goal(int steps,int speed);

//更改电机2的目标
void setMotor2Goal(int steps,int speed);

//更改电机3的目标
void setMotor3Goal(int steps,int speed);
#endif
中层c文件


#include "control.h"
#include "pid.h"
#include "encoder.h"
#include "motor.h"
#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "niming.h"
#include

//全局变量
MotorParm motor1,motor2,motor3; //三个电机的控制结构体

/**************************************************************************
*Name   ：MotorControl
*Funs   ：电机控速函数
*Input  ：GoalSpeed 目标速度，范围需要根据电机而定
*Output ：None
**************************************************************************/
IncrementalPid pid;
void MotorControl(float GoalSpeed){
    pid.Error=0;
    pid.LastError1=0;
    pid.LastError2=0;
       
    pid.Flag_First=1;
    pid.MinOutput=-1;
    pid.MaxOutput=1;
    pid.NoneActValue=0;
       
    pid.GoalVale = GoalSpeed;
    pid.Kp=-0.00003;
    pid.Ki=0;
    pid.Kd=0;
       
    getSpeed(1,1);
    float speed;
    float rate=0;
    float temp;
    u8 buf1[4],buf2[4];
    u8 *temp1,*temp2;
    while(1){
        speed = getSpeed(1,1);
        incrementalPid_Cal(&pid,speed);
        rate+=pid.Output;
        SetRate(rate,1);
        temp = pid.GoalVale;
        temp1 = (u8*)&(temp);
        temp2 = (u8*)&speed;
        for(int i=0;i<4;i++){
            buf1 = *(temp1+3-i);
            buf2 = *(temp2+3-i);
        }
            delay_us(1000);
        }
}

void setMotorSpeed(float speed){
    pid.GoalVale = speed;
    printf("%frn",pid.GoalVale);
}

void limitValue(float* rate,float min,float max){
    if(*rate>0&&*rate         *rate = min;
    }
    if(*rate<0&&*rate>-min){
        *rate = -min;
    }
    if(*rate>0&&*rate > max){
            *rate = max;
    }
    if(*rate<0&&*rate < -max){
        *rate = -max;
    }
}


/**************************************************************************
*Name   ：motorProcessControl
*Funs   ：电机控速控距离处理函数:分为两个步骤，先调速，再调
*Input  ：motor 是电机控制的结构体指针
*Output ：None
**************************************************************************/
void motorProcessControl(MotorParm *motor){
//        if(motor->flag_finish==1)
//                return; //如果任务完成，则啥都不干
    if(motor->flag_state==1){//调速
            motorSpeed(motor);
    }else if(motor->flag_state==2){//调距
            motorDistance(motor);
    }
}

/**************************************************************************
*Name   ：motorSpeed
*Funs   ：电机调速模块，这里对电机速度进行控制
*Input  ：motor 是电机控制的结构体指针
*Output ：None
**************************************************************************/
void motorSpeed(MotorParm *motor){
    motor->speed = getSpeed(0,motor->number); //获取速度
    motor->dis -= motor->speed;
    positionalPid_Cal(&motor->pid_speed_p,motor->speed);
    motor->rate=motor->pid_speed_p.Output;
    limitValue(&motor->rate,MINRATE,1);
    SetRate(motor->rate,motor->number);
}

/**************************************************************************
*Name   ：motorDistance
*Funs   ：电机距离调节，调节电机的转动距离
*Input  ：motor 是电机控制的结构体指针
*Output ：None
**************************************************************************/
void motorDistance(MotorParm *motor){
    motor->dis += getSpeed(0,motor->number); //累加距离
    positionalPid_Cal(&motor->pid_dis,motor->dis);
    motor->rate=motor->pid_dis.Output;
    limitValue(&motor->rate,MINRATE,1);
    SetRate(motor->rate,motor->number);
}


/**************************************************************************
*Name   ：motorStateUpdata
*Funs   ：电机状态更新
*Input  ：motor 是电机控制的结构体指针
*Output ：None
**************************************************************************/
void motorStateUpdata(MotorParm *motor){
    if(motor->flag_state==1){ // 当前为速度调节
        if((motor->dis>0&&motor->disdis<0&&motor->dis>-STATETHRE)){
            motor->flag_state=2; //进入调距
        }
    }else if(motor->flag_state==2){
        if(motor->dis==0){
//                 motor->flag_state=0;
            motor->flag_finish=1;
        }
    }
}

/**************************************************************************
*Name   ：MotorsControlInit
*Funs   ：初始化电机控制结构体参数
*Input  ：motor 是电机控制的结构体指针
*Output ：None
**************************************************************************/
void MotorsControlInit(void){
    motor1.number=1;
    motor1.flag_finish=1;
   
    motor1.pid_speed_p.Error=0;
    motor1.pid_speed_p.Flag_First=0;
    motor1.pid_speed_p.MinOutput=-1;
    motor1.pid_speed_p.MaxOutput=1;
    motor1.pid_speed_p.MAXInte =1;
    motor1.pid_speed_p.Integral=0;
    motor1.pid_speed_p.IntePartValue=30;
    motor1.pid_speed_p.NoneActValue=0;
    motor1.pid_speed_p.Kp=-0.5;
    motor1.pid_speed_p.Ki=-0.3;
    motor1.pid_speed_p.Kd=0;

    motor1.pid_dis.Error=0;
    motor1.pid_dis.LastError=0;
    motor1.pid_dis.Integral=0;
    motor1.pid_dis.Flag_First=0;
    motor1.pid_dis.MaxOutput=1;
    motor1.pid_dis.MinOutput=-1;
    motor1.pid_dis.IntePartValue=100;
    motor1.pid_dis.MAXInte=0.5;
    motor1.pid_dis.NoneActValue=0;
   
    motor1.dis =0;
    motor1.pid_dis.GoalVale=0;
    motor1.pid_dis.Kp=-0.1;
    motor1.pid_dis.Ki=-0.3;
    motor1.pid_dis.Kd=-0.5;
    motor1.pid_speed_p.GoalVale=0;
   
    motor2.number=2;
    motor2.flag_finish=1;
   
    motor2.pid_speed_p.Error=0;
    motor2.pid_speed_p.Flag_First=0;
    motor2.pid_speed_p.MinOutput=-1;
    motor2.pid_speed_p.MaxOutput=1;
    motor2.pid_speed_p.MAXInte =1;
    motor2.pid_speed_p.Integral=0;
    motor2.pid_speed_p.IntePartValue=30;
    motor2.pid_speed_p.NoneActValue=0;
    motor2.pid_speed_p.Kp=-0.5;
    motor2.pid_speed_p.Ki=-0.3;
    motor2.pid_speed_p.Kd=0;

    motor2.pid_dis.Error=0;
    motor2.pid_dis.LastError=0;
    motor2.pid_dis.Integral=0;
    motor2.pid_dis.Flag_First=0;
    motor2.pid_dis.MaxOutput=1;
    motor2.pid_dis.MinOutput=-1;
    motor2.pid_dis.IntePartValue=100;
    motor2.pid_dis.MAXInte=0.5;
    motor2.pid_dis.NoneActValue=0;
   
    motor2.dis =0;
    motor2.pid_dis.GoalVale=0;
    motor2.pid_dis.Kp=-0.1;
    motor2.pid_dis.Ki=-0.3;
    motor2.pid_dis.Kd=-0.5;
    motor2.pid_speed_p.GoalVale=0;
   
    motor3.number=3;
    motor3.flag_finish=1;
   
    motor3.pid_speed_p.Error=0;
    motor3.pid_speed_p.Flag_First=0;
    motor3.pid_speed_p.MinOutput=-1;
    motor3.pid_speed_p.MaxOutput=1;
    motor3.pid_speed_p.MAXInte =1;
    motor3.pid_speed_p.Integral=0;
    motor3.pid_speed_p.IntePartValue=30;
    motor3.pid_speed_p.NoneActValue=0;
    motor3.pid_speed_p.Kp=-0.5;
    motor3.pid_speed_p.Ki=-0.3;
    motor3.pid_speed_p.Kd=0;
    motor3.pid_speed_p.GoalVale=0;   
    motor3.pid_dis.Error=0;
    motor3.pid_dis.LastError=0;
    motor3.pid_dis.Integral=0;
    motor3.pid_dis.Flag_First=0;
    motor3.pid_dis.MaxOutput=1;
    motor3.pid_dis.MinOutput=-1;
    motor3.pid_dis.IntePartValue=100;
    motor3.pid_dis.MAXInte=0.5;
    motor3.pid_dis.NoneActValue=0;
   
    motor3.dis =0;
    motor3.pid_dis.GoalVale=0;
    motor3.pid_dis.Kp=-0.1;
    motor3.pid_dis.Ki=-0.3;
    motor3.pid_dis.Kd=-0.5;
   
       
    motor1.pid_dis.MaxOutput=0.5;
    motor1.pid_dis.MinOutput=-0.5;
   
    motor2.pid_dis.MaxOutput=0.5;
    motor2.pid_dis.MinOutput=-0.5;
       
    motor3.pid_dis.MaxOutput=0.5;
    motor3.pid_dis.MinOutput=-0.5;
}

/**************************************************************************
*Name   ：setMotor1Goal
*Funs   ：设置(修改)电机1的控制目标
*Input  ：motor 是电机控制的结构体指针
*Output ：None
**************************************************************************/
void setMotor1Goal(int steps,int speed){
        motor1.flag_finish=0;
        motor1.flag_state=1; //速度控制
        motor1.dis = steps;
        motor1.pid_speed_p.GoalVale = speed;
}

/**************************************************************************
*Name   ：setMotor2Goal
*Funs   ：设置(修改)电机2的控制目标
*Input  ：motor 是电机控制的结构体指针
*Output ：None
**************************************************************************/
void setMotor2Goal(int steps,int speed){
    motor2.flag_finish=0;
    motor2.flag_state=1; //速度控制
    motor2.dis = steps;
    motor2.pid_speed_p.GoalVale = speed;
}

/**************************************************************************
*Name   ：setMotor3Goal
*Funs   ：设置(修改)电机3的控制目标
*Input  ：motor 是电机控制的结构体指针
*Output ：None
**************************************************************************/
void setMotor3Goal(int steps,int speed){
    motor3.flag_finish=0;
    motor3.flag_state=1; //速度控制
    motor3.dis = steps;
    motor3.pid_speed_p.GoalVale = speed;
}
这份代码非常容易控制成控制n个电机的代码





5、步进电机控速控距
我这里使用的SPTA算法来进行加减速的控制，同时控制三个步进电机，可以很容易地扩展成控制任意个步进电机，类似直流控制，步进控制也分为底层的SPTA控制和中层的控制。我的代码做到了可以中途任意变速，但是没有做到中途任意变距离。


底层头文件


#ifndef _MYSPTA_H
#define _MYSPTA_H
#include "sys.h"
typedef volatile int SPTAVAR;

//脉冲1
#define STEP1 PFout(0)

//脉冲2
#define STEP2 PFout(2)

//脉冲3
#define STEP3 PFout(4)

//电机1正方向
#define POSITIVE1 {PFout(1)=0;motor1.Direction=1;}
//电机1负方向
#define NEGATIVE1 {PFout(1)=1;motor1.Direction=0;}

//电机2正方向
#define POSITIVE2 {PFout(3)=0;motor2.Direction=1;}
//电机2负方向
#define NEGATIVE2 {PFout(3)=1;motor2.Direction=0;}

//电机3正方向
#define POSITIVE3 {PFout(5)=0;motor3.Direction=1;}
//电机3负方向
#define NEGATIVE3 {PFout(5)=1;motor3.Direction=0;}

//速度最小50 最大50000
//暂定加速度400000，在600000左右达到极致
#define MAXACCELERATION 1000000
#define SPTAOPEN  {TIM_Cmd(TIM3,ENABLE);TIM_Cmd(TIM4,ENABLE);TIM_Cmd(TIM5,ENABLE);}
#define SPTACLOSE {TIM_Cmd(TIM3,DISABLE);TIM_Cmd(TIM4,DISABLE);TIM_Cmd(TIM5,DISABLE);}
#define SPTACLOSE1 {TIM_Cmd(TIM3,DISABLE);}
#define SPTACLOSE2 {TIM_Cmd(TIM4,DISABLE);}
#define SPTACLOSE3 {TIM_Cmd(TIM5,DISABLE);}

typedef struct STEPMOTOR{
        SPTAVAR PosAccumulator;     //位置累加器
        SPTAVAR PosAdd;                          //位置增加值
        SPTAVAR ActualPosition;                  //实际位置
        SPTAVAR TargetPosion;                    //目标位置，用户输入步进电机运动的步数
       
        SPTAVAR VelAccumulator;     //速度累加器
        SPTAVAR ActualAcceleration; //实际加速度，用户设定的加速度数值
        SPTAVAR VelAdd;             //速度增加值
        SPTAVAR ActualVelocity;     //实际速度
        SPTAVAR TargetVelocity;     //目标速度
       
        u8 SPTAState;                                                                //SPTA的状态：0 空闲 | 1 加速 | 2 匀速 | 3 减速
       
        SPTAVAR FinishFlag;                                        //状态完成标志，每当一个状态完成时，将它置1，其清零操作由应用层手动清零
        SPTAVAR StepFinishFlag;                 //步数完成标志
        SPTAVAR Direction;                                        //方向标志  1:正方向 0：负方向
        u8 Mode;                                                                                //STPA工作模式选择 0：速度调控模式 1：步数调控模式
        int AccelerationSteps;                        //加速步数
        int Myposition;                                                        //系统位置
        u8 number;                                                             //电机的编号
}StepMotor;


//电机结构参数
extern StepMotor motor1,motor2,motor3;

//初始化
void sptaInit(void);

//速度更新
void velUpdate(StepMotor *motor);

//位置更新
void posUpdate(StepMotor *motor);

//状态调度
void dealState(StepMotor *motor);

//更改状态
void stateTurn(StepMotor *motor,int);

//停止工作判断
void ReadySTop(void);

#endif
底层的c文件


#include "mySPTA.h"
#include "sys.h"
#include "delay.h"
//全局变量--两个电机结构体参数
StepMotor motor1,motor2,motor3;

/**********************************************************
*Name   :sptaInit
*Funs   :初始化SPTA所需的定时器，脉冲输出IO
*Input  :None
*Output :None
***********************************************************/
void sptaInit(void){
    //初始化定时器
    u16 arr=200-1,psc=84-1;  //10KHz
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);  ///使能TIM3时钟
   
    TIM_TimeBaseInitStructure.TIM_Period = arr;         //自动重装载值
    TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
    TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
       
    TIM_Cmd(TIM3,DISABLE); //关闭定时器3
    TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStructure);//初始化TIM3
    TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE); //允许定时器3更新中断
       
    NVIC_InitStructure.NVIC_IRQChannel=TIM3_IRQn; //定时器3中断
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
    NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3
    NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
    NVIC_Init(&NVIC_InitStructure);
       

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);  ///使能TIM4时钟
    TIM_TimeBaseInitStructure.TIM_Period = arr;         //自动重装载值
    TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
    TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
       
    TIM_Cmd(TIM4,DISABLE); //关闭定时器4
    TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure);//初始化TIM4
    TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE); //允许定时器4更新中断
       
    NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn; //定时器4中断
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
    NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3
    NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5,ENABLE);  ///使能TIM5时钟
       
    TIM_TimeBaseInitStructure.TIM_Period = arr;         //自动重装载值
    TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  //定时器分频
    TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
    TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
       
    TIM_Cmd(TIM5,DISABLE); //关闭定时器5
    TIM_TimeBaseInit(TIM5,&TIM_TimeBaseInitStructure);//初始化TIM5
    TIM_ITConfig(TIM5,TIM_IT_Update,ENABLE); //允许定时器5更新中断
       
    NVIC_InitStructure.NVIC_IRQChannel=TIM5_IRQn; //定时器5中断
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
    NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3
    NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
    NVIC_Init(&NVIC_InitStructure);
       
    //初始化控制引脚
    GPIO_InitTypeDef  GPIO_InitStructure;
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE);//使能GPIOF时钟
    //GPIOF0-3初始化设置
      GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1| GPIO_Pin_2| GPIO_Pin_3|GPIO_Pin_4| GPIO_Pin_5;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;//普通输出模式
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;//推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100MHz
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;//上拉
    GPIO_Init(GPIOF, &GPIO_InitStructure);//初始化
    GPIO_ResetBits(GPIOF,GPIO_Pin_0 | GPIO_Pin_1| GPIO_Pin_2| GPIO_Pin_3|GPIO_Pin_4| GPIO_Pin_5);//默认低
       
    motor1.SPTAState=0;   //电机1状态，空闲  
    motor2.SPTAState=0;   //电机2状态，空闲
    motor3.SPTAState=0;
       
    motor1.FinishFlag=1;  //电机1结束标志，空闲
    motor2.FinishFlag=1;  //电机2结束标志，空闲
    motor3.FinishFlag=1;
       
    motor1.number=1;
    motor2.number=2;
    motor3.number=3;
}


/****************************************************************
*Name   :TIM3_IRQHandler
*Funs   :定时器3中断函数，速度更新，位置更新，IO输出
*Input  :None
*Output :None
*****************************************************************/
void TIM3_IRQHandler(void){
    if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET){
            velUpdate(&motor1);  //速度更新
            posUpdate(&motor1);  //位置更新
            dealState(&motor1);  //状态
            if(motor1.SPTAState==0){
            SPTACLOSE1;
        }
    }
    TIM_ClearITPendingBit(TIM3,TIM_IT_Update);//清楚中断标志
}

/****************************************************************
*Name   :TIM4_IRQHandler
*Funs   :定时器4中断函数，速度更新，位置更新，IO输出
*Input  :None
*Output :None
*****************************************************************/
void TIM4_IRQHandler(void){
    if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET){
        velUpdate(&motor2);  //速度更新
        posUpdate(&motor2);  //位置更新
        dealState(&motor2);  //状态
        if(motor2.SPTAState==0){
            SPTACLOSE2;
        }
    }
    TIM_ClearITPendingBit(TIM4,TIM_IT_Update);//清楚中断标志
}

/****************************************************************
*Name   :TIM5_IRQHandler
*Funs   :定时器5中断函数，速度更新，位置更新，IO输出
*Input  :None
*Output :None
*****************************************************************/
void TIM5_IRQHandler(void){
    if(TIM_GetITStatus(TIM5,TIM_IT_Update)==SET){
        velUpdate(&motor3);  //速度更新
        posUpdate(&motor3);  //位置更新
        dealState(&motor3);  //状态
        if(motor3.SPTAState==0){
            SPTACLOSE3;
        }
    }
    TIM_ClearITPendingBit(TIM5,TIM_IT_Update);//清楚中断标志
}

/************************************************************
*Name   :velUpdate
*Funs   :速度更新
*Input  :None
*Output :None
*************************************************************/
void velUpdate(StepMotor *motor){
    if(motor->ActualVelocity!=motor->TargetVelocity){
        motor->VelAccumulator+=motor->ActualAcceleration;    //速度累加器+实际加速度
        motor->VelAdd = motor->VelAccumulator>>17;           //右移16位，判断速度累加器是否溢出
        motor->VelAccumulator -= motor->VelAdd << 17;        //如果溢出，则速度累加器去掉溢出部分
        if(motor->ActualVelocityTargetVelocity){
            motor->ActualVelocity = (motor->ActualVelocity+motor->VelAdd)TargetVelocity?(motor->ActualVelocity+motor->VelAdd):motor->TargetVelocity;
        }else if(motor->ActualVelocity>motor->TargetVelocity){
            motor->ActualVelocity = (motor->ActualVelocity-motor->VelAdd)>motor->TargetVelocity?(motor->ActualVelocity-motor->VelAdd):motor->TargetVelocity;
        }
    }else{
        motor->VelAccumulator=0;
        motor->VelAdd=0;
    }
}

void stepOne(u8 which){
    if(which==1){//电机1
        STEP1 =1;                       //产生一个脉冲
        for(int j=0;j<100;j++) //经过测试，84个脉冲接近5us，或2.5us
            __nop();
                STEP1=0;
        }else if(which==2){//电机2
            STEP2 =1;                       //产生一个脉冲
            for(int j=0;j<100;j++) //经过测试，84个脉冲接近5us，或2.5us
                __nop();
            STEP2=0;
    }else if(which==3){
        STEP3 =1;                       //产生一个脉冲
        for(int j=0;j<100;j++) //经过测试，84个脉冲接近5us，或2.5us
                __nop();
        STEP3=0;
    }
}

/**************************************************************************
*Name   :posUpdate
*Funs   :位置更新
*Input  :None
*Output :None
***************************************************************************/
void posUpdate(StepMotor *motor){
    motor->PosAccumulator+=motor->ActualVelocity;  //位置累加器+实际速度
    motor->PosAdd = motor->PosAccumulator >> 17;   //左移17位，判断是否溢出
    motor->PosAccumulator -= motor->PosAdd << 17;  //如果位置累加器溢出，则去掉溢出部分
    if(motor->PosAdd!=0){
        if(motor->Mode){ //计步模式
            if(motor->SPTAState==1){
                motor->AccelerationSteps++;
                if(motor->AccelerationSteps>(motor->TargetPosion/2)){//转至减速状态
                    motor->TargetVelocity=0;
                    stateTurn(motor,3);
                }
            }else if(motor->SPTAState==2){
                if((--motor->TargetPosion)<=0){ //匀速过程完毕，开始减速至0
                    motor->TargetVelocity=0;
                    stateTurn(motor,3);
            }
        }
    }
    stepOne(motor->number);
    if(motor->Direction)
        motor->Myposition++;
    else
        motor->Myposition--;
    }
}

/*****************************************************************
*Name   :dealState
*Funs   :状态调度
*Input  :motor表示电机参数结构体
*Output :None
******************************************************************/
void dealState(StepMotor *motor){
    if((motor->SPTAState==1||motor->SPTAState==3)&&(motor->ActualVelocity==motor->TargetVelocity)){//如果加减速，却实际速度等于目标速度
        motor->FinishFlag=1;    //将完成标志置1
        if(motor->SPTAState==3&&motor->ActualVelocity==0){//空闲状态
            stateTurn(motor,0);
        }else{
            stateTurn(motor,2);
        }
    }else if(motor->SPTAState==2&&(motor->ActualVelocity!=motor->TargetVelocity)){//如果匀速，却实际速度不等于目标速度
        motor->FinishFlag=1;    //将完成标志置1
        if(motor->ActualVelocity>motor->TargetVelocity)
            stateTurn(motor,3);//减速
        else
            stateTurn(motor,1);//加速
        }else if(motor->SPTAState==0){ //空闲
            stateTurn(motor,0);
    }
}

/*****************************************************************
*Name   :stateTurn
*Funs   :更改电机状态
*Input  :State 表示要转成的状态 1：加速 2：匀速 3：减速;motor是电机参数结构体
*Output :None
******************************************************************/
void stateTurn(StepMotor * motor,int State){
    if(motor->Mode&&motor->SPTAState==3){ //步数完成，清零加速过程的距离
        motor->AccelerationSteps=0;
        motor->StepFinishFlag=1;
    }
    motor->SPTAState=State;
    if(State==1){//加速
        motor->ActualAcceleration = MAXACCELERATION;
    }else if(State==2){//变为匀速
    if(motor->Mode){//计步
        motor->TargetPosion = motor->TargetPosion -2*motor->AccelerationSteps;
        if(motor->TargetPosion<0)
            motor->TargetPosion=0;
        }
        motor->ActualAcceleration = 0;
    }else if(State==3){//减速
        motor->ActualAcceleration = MAXACCELERATION;
    }else if(State==0){
        motor->ActualAcceleration = 0;
        motor->TargetVelocity=0;
        motor->TargetPosion=0;
    }
}


/*****************************************************************
*Name   :ReadySTop
*Funs   :判断定时器
*Input  :None
*Output :None
******************************************************************/
void ReadySTop(void){
    if(motor1.SPTAState==0&&motor2.SPTAState==0){//停止
        SPTACLOSE;
    }
}
中层头文件


#ifndef _SPTAAPP_H
#define _SPTAAPP_H
#include "sys.h"
#include "mySPTA.h"

//启动，以指定速度向前走
void sPTAStart(int vel1,int vel2,int vel3);

//停止
void sPTAStop();

//调速
void setVel(StepMotor* motor,int vel);

//向某个方向走指定步数，以指定速度
void sPTAGo(int Steps1,int vel1,int Steps2,int vel2,int Steps3,int vel3);
//急刹
void stop();

//复位
void toZero(void);
#endif
中层c文件


#include "mySPTA.h"
#include "SPTAAPP.h"
#include "usart.h"
#include "LED.h"
#include "delay.h"
#include "key.h"
#include

/*****************************************************************
*Name   :changeDir
*Funs   :更改电机方向
*Input  :which 表示哪一个电机，范围1-3；dir表示方向，0负方向，1正方向
*Output :None
******************************************************************/
static void changeDir(u8 which,u8 dir){
    if(which==1){
            if(dir==0){
            NEGATIVE1;
        }else if(dir==1){
                POSITIVE1;
        }
    }else if(which==2){
        if(dir==0){
            NEGATIVE2;
        }else if(dir==1){
            POSITIVE2;
        }
    }else if(which==3){
        if(dir==0){
            NEGATIVE3;
        }else if(dir==1){
            POSITIVE3;
        }
    }
}

/*****************************************************************
*Name   :sPTAStart
*Funs   :三个电机以一个指定的速度开始运动
*Input  :vel1表示电机1的速度 vel2表示电机2的速度 vel3表示电机3的速度
*Output :None
******************************************************************/
void sPTAStart(int vel1,int vel2,int vel3){
    if(!SYSTEMKEY)
        return;
    //电机1
    if(vel1>=0){
        motor1.TargetVelocity = vel1; //设置目标速度
        changeDir(motor1.number,1);
    }else{
        motor1.TargetVelocity = -vel1; //设置目标速度
        changeDir(motor1.number,0);
    }
    stateTurn(&motor1,1);         //加速
    //电机2
    if(vel2>=0){
        motor2.TargetVelocity = vel2; //设置目标速度
        changeDir(motor2.number,1);
    }else{
        motor2.TargetVelocity = -vel2; //设置目标速度
        changeDir(motor2.number,0);
    }
    stateTurn(&motor2,1);         //加速
    //电机3
    if(vel3>=0){
        motor3.TargetVelocity = vel3; //设置目标速度
        changeDir(motor3.number,1);
    }else{
        motor3.TargetVelocity = -vel3; //设置目标速度
        changeDir(motor3.number,0);
    }
    stateTurn(&motor3,1);         //加速
       
    //开始
    SPTAOPEN;
}

/**************************************************************
*Name   :sPTAStop
*Funs   :三个电机停止运动，三个电机停止后返回
*Input  :None
*Output :None
***************************************************************/
void sPTAStop(void){
    motor1.TargetVelocity=0; //电机1目标速度改为0
    stateTurn(&motor1,3);    //状态转为减速
    motor2.TargetVelocity=0; //电机2目标速度改为0
    stateTurn(&motor2,3);    //状态转为减速
    motor3.TargetVelocity=0; //电机3目标速度改为0
    stateTurn(&motor3,3);    //状态转为减速
    while(!((motor1.SPTAState==0)&&(motor2.SPTAState==0)&&(motor3.SPTAState==0)))
        ;
       
    //定时器关闭
    SPTACLOSE;
}

/****************************************************************
*Name   :setVel
*Funs   :将电机速度改为指定速度，反向时由于电机先减速至0然后再加速，所以会产生阻塞
*Input  :motor表示电机结构参数,vel 表示指定的速度
*Output :None
****************************************************************/
void setVel(StepMotor* motor,int vel){
    if(!SYSTEMKEY)
        return;
    if(vel==motor->TargetVelocity&&((vel>0&&motor->Direction)||(vel<0&&!motor->Direction)))  //等速
        return;
    if(vel>0&&!motor->Direction){
        motor->TargetVelocity=0; //目标速度改为0
        stateTurn(motor,3);     //状态转为减速
        motor->FinishFlag=0;
        SPTAOPEN;
        while(!motor->FinishFlag)
            ;
        changeDir(motor->number,1);
        motor->TargetVelocity = vel;
        stateTurn(motor,1);  //加速
    }else if(vel<0&&motor->Direction){
        motor->TargetVelocity=0; //目标速度改为0
        stateTurn(motor,3);     //状态转为减速
        motor->FinishFlag=0;
        SPTAOPEN;
        while(!motor->FinishFlag)
            ;
        changeDir(motor->number,0); //电机方向改为负
        motor->TargetVelocity = -vel;
        stateTurn(motor,1);  //加速
    }else if(vel>0){
        motor->TargetVelocity = vel;
        if(vel>motor->TargetVelocity){
            stateTurn(motor,1);      //加速
        }else if(velTargetVelocity){
            stateTurn(motor,3);      //减速
        }
    }else if(vel<0){
        motor->TargetVelocity = -vel;
        if(-vel>motor->TargetVelocity){
            stateTurn(motor,3);      //加速
        }else if(-velTargetVelocity){
            stateTurn(motor,1);      //减速
        }
    }else if(vel==0){
        motor->TargetVelocity=0; //目标速度改为0
        stateTurn(motor,3);     //状态转为减速
    }
}


/******************************************************************
*Name   :sPTAGo
*Funs   :两个电机向一个方向走固定步数
*Input  :Steps1 vel1 表示电机1的步数和速度，Steps2 vel2 表示电机2的步数和速度,Steps3 vel3 表示电机3的步
                                 数和速度 ，其中steps的正负表示方向，velx为正数
*Output :None
******************************************************************/
void sPTAGo(int Steps1,int vel1,int Steps2,int vel2,int Steps3,int vel3){
    if(!SYSTEMKEY)
        return;
    motor1.Mode=1;   //步数调控模式
    motor2.Mode=1;   //步数调控模式
    motor3.Mode=1;   //步数调控模式
    if(Steps1>=0){
        changeDir(motor1.number,1);
    }else{
        changeDir(motor1.number,0);
        Steps1 = -Steps1;
    }
    if(Steps2>=0){
        changeDir(motor2.number,1);
    }else{
        changeDir(motor2.number,0);
        Steps2 = -Steps2;
    }
    if(Steps3>=0){
        changeDir(motor3.number,1);
    }else{
        changeDir(motor3.number,0);
        Steps3 = -Steps3;
    }
    motor1.AccelerationSteps=0;
    motor1.TargetVelocity=vel1;
    motor1.TargetPosion = Steps1;
    stateTurn(&motor1,1);
       
    motor2.AccelerationSteps=0;
    motor2.TargetVelocity=vel2;
    motor2.TargetPosion = Steps2;
    stateTurn(&motor2,1);
       
    motor3.AccelerationSteps=0;
    motor3.TargetVelocity=vel3;
    motor3.TargetPosion = Steps3;
    stateTurn(&motor3,1);

    SPTAOPEN;
    while(!((motor1.SPTAState==0)&&(motor2.SPTAState==0)&&(motor3.SPTAState==0))){
            __nop();
    }
    SPTACLOSE;
    motor1.Mode=0;
    motor2.Mode=0;
    motor3.Mode=0;
}

/*********************************************************************
*Name   :toZero
*Funs   :复位
*Input  :None
*Output :None
************************************************************************/
void toZero(void){
    if(SYSTEMKEY&&LIMIT1)
        ;
}

/************************************************************************
*Name   :stop
*Funs   :直接停下
*Input  :None
*Output :None
**************************************************************************/
void stop(){
    motor1.TargetVelocity=0;
    motor2.TargetVelocity=0;
    motor3.TargetVelocity=0;
    motor1.SPTAState=0;
    motor2.SPTAState=0;
    motor3.SPTAState=0;
    SPTACLOSE;
}