2019年电赛准备程序STM32版本
2024-04-24 14:09:21
现在是2019年8月11号，昨天电赛结束，不愿意好的东西随时间而消失不见，所以将准备的程序总结一下，分享出来。
目录
一、电赛的故事
二、准备程序
1、PID
2、滤波
3、菜单
4、直流编码电机控速和控距
5、步进电机控速控距
一、电赛的故事
这是我的第三次电赛，大一炮灰，比较轻松，大二大三就很累，四天基本只能睡6个小时，所以今天从2点睡到了17点。由于今年的电赛比较诡异，没有纯粹的控制题，所以我这个做控制的比较被动，选择的电磁炮题。
二、准备程序
三次电赛准备了很多的程序，这里将一些常用的程序分享出来。
1、PID
控制题的必备品，谁用谁知道
头文件pid.h
#ifndef _PID_H
#define _PID_Htypedef 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);#endifc文件
#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);elsepid->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 5typedef 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);#endifc文件
#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;elsePOINT_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; //100MHzGPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉GPIO_Init(GPIOF, &GPIO_InitStructure); //初始化GPIOMOTOR1_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复用为定时器3GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM3); //GPIOC7复用为定时器3GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM3); //GPIOC8复用为定时器3GPIO_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; //速度100MHzGPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉GPIO_Init(GPIOC,&GPIO_InitStructure); //初始化PC678TIM_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脉冲宽度调制模式2TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低TIM_OC1Init(TIM3, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM3 OC1TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIM3在CCR1上的预装载寄存器 //初始化TIM3 Channel2 PWM模式 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性TIM_OC2Init(TIM3, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM3 OC2TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIM3在CCR2上的预装载寄存器 //初始化TIM3 Channel3 PWM模式 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低TIM_OC3Init(TIM3, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM3 OC3TIM_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 30typedef struct MOTORPARM{//此结构作为外部控制的接口int speed; //电机当前速度int dis; //电机当前目标距离u8 flag_finish; //动作完成标志u8 flag_state; //调速或调距u8 number; //电机编号float rate ; //电机占空比IncrementalPid pid_speed; //电机运算pidPositionalPid pid_speed_p;//电机位置式速度pidPositionalPid 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; //10KHzTIM_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); //关闭定时器3TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStructure);//初始化TIM3TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE); //允许定时器3更新中断NVIC_InitStructure.NVIC_IRQChannel=TIM3_IRQn; //定时器3中断NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3NVIC_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); //关闭定时器4TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure);//初始化TIM4TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE); //允许定时器4更新中断NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn; //定时器4中断NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3NVIC_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); //关闭定时器5TIM_TimeBaseInit(TIM5,&TIM_TimeBaseInitStructure);//初始化TIM5TIM_ITConfig(TIM5,TIM_IT_Update,ENABLE); //允许定时器5更新中断NVIC_InitStructure.NVIC_IRQChannel=TIM5_IRQn; //定时器5中断NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3NVIC_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;//100MHzGPIO_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){//电机1STEP1 =1; //产生一个脉冲for(int j=0;j<100;j++) //经过测试，84个脉冲接近5us，或2.5us__nop();STEP1=0;}else if(which==2){//电机2STEP2 =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){ //匀速过程完毕，开始减速至0motor->TargetVelocity=0;stateTurn(motor,3);}}}stepOne(motor->number);if(motor->Direction)motor->Myposition++;elsemotor->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; //将完成标志置1if(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; //将完成标志置1if(motor->ActualVelocity>motor->TargetVelocity)stateTurn(motor,3);//减速elsestateTurn(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;//电机1if(vel1>=0){motor1.TargetVelocity = vel1; //设置目标速度changeDir(motor1.number,1);}else{motor1.TargetVelocity = -vel1; //设置目标速度changeDir(motor1.number,0);}stateTurn(&motor1,1); //加速//电机2if(vel2>=0){motor2.TargetVelocity = vel2; //设置目标速度changeDir(motor2.number,1);}else{motor2.TargetVelocity = -vel2; //设置目标速度changeDir(motor2.number,0);}stateTurn(&motor2,1); //加速//电机3if(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目标速度改为0stateTurn(&motor1,3); //状态转为减速motor2.TargetVelocity=0; //电机2目标速度改为0stateTurn(&motor2,3); //状态转为减速motor3.TargetVelocity=0; //电机3目标速度改为0stateTurn(&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; //目标速度改为0stateTurn(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; //目标速度改为0stateTurn(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; //目标速度改为0stateTurn(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;
}当然，同样的，这里只是把核心代码贴出来了，如果需要完整的代码，可以看我最后面给出的链接，花一些C币购买，也可以关注我的微信公众号 “山人彤” ，回复 “STM32电赛准备“ 即可免费获取
当然，如果有问题，可以加我好友
qq:1826249828
完整的2019年电赛准备的32工程链接：https://download.csdn.net/download/qq_39545674/11523012
完整的2018年电赛准备的32工程链接：https://download.csdn.net/download/qq_39545674/11523033
2019年电赛准备程序STM32版本相关推荐
- 【立创EDA开源推荐】005期 | 2019年电赛H题 电磁炮（激光版）
工程名称: 2019年电赛H题 电磁炮(激光版) 工程作者:zzxw 工程主页链接: https://oshwhub.com/kakaka/tu-ya-zhi-neng-yun-duo 开源协议: G ...
- 2019年电赛国赛D题《简易电路特性测试仪》训练总结----题目引入与概述
系列文章链接目录 一.2017年电赛国赛H题<远程幅频特性测试装置>训练总结(DDS信号源部分) 二.2017年电赛国赛H题<远程幅频特性测试装置>训练总结(放大器部分) 三. ...
- 2019年电赛国赛D题《简易电路特性测试仪》训练总结----待测三极管共射放大电路部分
系列文章链接目录 一.2017年电赛国赛H题<远程幅频特性测试装置>训练总结(DDS信号源部分) 二.2017年电赛国赛H题<远程幅频特性测试装置>训练总结(放大器部分) 三. ...
- 2019年电赛D题《简易电路特性测试仪》全过程
本人为团队中负责硬件部分,为了准备2022年电赛,本队伍已经制作2019年和2021年电赛信号题,本次主要讲解为2019年电赛D题硬件部分,少部分为软件需要做的部分.后续会对整个硬件进行优化和整理. ...
- 2019年电赛综合测评题详解
2019年全国大学生电子设计竞赛综合测评已经结束,邀请到西电研究生李天红同学给大家做重点分析. 首先看题目: 视频要点提示:题目分析.常用波形变换电路.两种可行方案.方案仿真.实际过程中遇到的问题分析 ...
- [随笔]2019省电赛无人机题（B题）总结
2019/8/7~2019/8/10在江苏赛区参加了省电子设计竞赛,这次也是省电赛和全国电赛重合的一年 选择的题目是无人机题 先说结果吧,直接白给了,挺遗憾的 电赛结束挺久了,作为一个从飞行器设计转到 ...
- 2019年电赛电磁炮，冲刺国赛
先自报家门吧,本人大二,来自双一流大学,具体那个大学就不介绍了,在大一的时候就进入了专业实验室,本参与实验的培训,学习了很多关于单片机的知识,在今年,学校组织参加电赛,经过两轮的重重筛选,进入了最后的 ...
- 2019全国电赛总结
准备阶段: 赛前大约一个星期就开始全力备战电赛,由于学校条件不算太好,所以选题上就不选放大器设计之类的题目,只能选择一些电源题,控制题,造飞机题. 技术准备: 这里推荐一下ti的webench,是专门 ...
- 单片机STM8S测量电压电路_纸张计数测量显示装置+【2019年电赛F题国一获奖作品】...
一 题目要求: 二 设计方案 1.硬件部分 硬件部分的制作,当初我们组内讨论了三套方案 用FCD2214芯片去采,两极板之间的电容,通过FCD2214转化为AD值,经过测试,发现FCD2214的值,受 ...
最新文章
- python上下条形图-Python之多变量叠加条形图
- IO之阻塞与非阻塞比较
- EleemntUI中el-table的formatter格式化字典显示的使用
- 蓝色版苹果iPhone 12开箱上手视频流出；谷歌回应司法部反垄断诉讼：存在严重漏洞；Git 2.29 稳定版发布|极客头条
- fp-tree算法matlab实现,现代设计优化算法MATLAB实现
- 中国股市低迷的根本原因
- Automatic Tumor Segmentation from MRI scans 阅读笔记
- android分享到新浪微博，认证+发送微博
- 计算机视觉的发展历程
- ppp协议c语言,ppp协议是用于拨号上网和路由器之间通信的点到点通信协议，是属于（1）协议，它不具有（2）的功能。（ - 信管网...
- html设置为壁纸win10,win10系统怎么设置桌面壁纸
- 网页文字提取插件-网页文字提取器
- cad帧数测试软件,CAD打开很卡该怎么办?快速提高cad运行速度的技巧
- LSTM之父最新长文：现代AI和深度学习发展史
- java aes解密源码_java AES解密，用易语言实现
- 低调的华丽：从服务器开发的角度认识 asp.net 中的回调技术
- 非对称加密算法--RSA
- selenium通过cookie跳过登录验证码实现登录
- OJ 1168 改写整数
- 【电脑使用】利用diskpart删除电脑的EFI分区