2019年電賽準備程序STM32版本

現在是2019年8月11號，昨天電賽結束，不願意好的東西隨時間而消失不見，所以將準備的程序總結一下，分享出來。

目錄

一、電賽的故事

二、準備程序

1、PID

2、濾波

3、菜單

4、直流編碼電機控速和控距

5、步進電機控速控距

---

一、電賽的故事

這是我的第三次電賽，大一炮灰，比較輕鬆，大二大三就很累，四天基本只能睡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 <math.h>

/*************************************************************************
*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 <stdlib.h>

/******************************************************************
*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[i];
        }
        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[i];
	}
	return res / SIZE;
    }
}

/**************************************************************
*Name	:MiddleFilter
*Funs	:滑動中值濾波
*Input	:f表示濾波器，data爲濾波器的輸入數據
*Output :None
***************************************************************/
Element MiddleFilter(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[i];
	}
	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[i] = f->Data[i];
	    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 <stdlib.h>
#include <stdio.h>
#include <string.h>
#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;i<item->CurrentChild;i++){
    	sprintf(buf,"%d  %s",i+1,item->ChildrenMenus[i]->name);
    	POINT_COLOR=RED;
    	LCD_DrawRectangle(XBais-5,YBais+i*YAdd-5,XBais+200,YBais+i*YAdd+25);
    	if(item->ChildrenMenus[i]->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 <math.h>

//全局變量
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[i] = *(temp1+3-i);
	    buf2[i] = *(temp2+3-i);
	}
	    delay_us(1000);
	}
}

void setMotorSpeed(float speed){
    pid.GoalVale = speed;
    printf("%f\r\n",pid.GoalVale);
}

void limitValue(float* rate,float min,float max){
    if(*rate>0&&*rate<min){
	*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->dis<STATETHRE)||(motor->dis<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->ActualVelocity<motor->TargetVelocity){
	    motor->ActualVelocity = (motor->ActualVelocity+motor->VelAdd)<motor->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 <math.h>

/*****************************************************************
*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(vel<motor->TargetVelocity){
	    stateTurn(motor,3);      //減速
        }
    }else if(vel<0){
	motor->TargetVelocity = -vel;
	if(-vel>motor->TargetVelocity){
	    stateTurn(motor,3);      //加速
	}else if(-vel<motor->TargetVelocity){
	    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;
}

當然，同樣的，這裏只是把核心代碼貼出來了，如果需要完整的代碼，可以看我最後面給出的鏈接

當然，如果有問題，可以加我好友

qq:1826249828