现在是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;
}
当然，同样的，这里只是把核心代码贴出来了，如果需要完整的代码，可以看我最后面给出的链接，花一些C币购买，也可以关注我的微信公众号 “山人彤” ，回复 “STM32电赛准备“ 即可免费获取
当然，如果有问题，可以加我好友
qq:1826249828
完整的2019年电赛准备的32工程链接：https://download.csdn.net/download/qq_39545674/11523012
完整的2018年电赛准备的32工程链接：https://download.csdn.net/download/qq_39545674/11523033
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
