Chen Huan / Mbed 2 deprecated MotorPID

闭环步进电机

Dependencies:   mbed

motor.h@0:5b4f19f8cd85, 2018-03-30 (annotated)

Committer:
heroistired
Date:
Fri Mar 30 12:03:12 2018 +0000
Revision:
0:5b4f19f8cd85
??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
heroistired 0:5b4f19f8cd85 1 #ifndef __MOTOR_H
heroistired 0:5b4f19f8cd85 2 #define __MOTOR_H
heroistired 0:5b4f19f8cd85 3 #include "mbed.h"
heroistired 0:5b4f19f8cd85 4
heroistired 0:5b4f19f8cd85 5 #define MotorKp 1
heroistired 0:5b4f19f8cd85 6 #define MotorKi 0
heroistired 0:5b4f19f8cd85 7 #define MotorVKp 50
heroistired 0:5b4f19f8cd85 8 #define MotorVKi 0
heroistired 0:5b4f19f8cd85 9
heroistired 0:5b4f19f8cd85 10 #define MotorKp_End 1
heroistired 0:5b4f19f8cd85 11 #define MotorKi_End 0
heroistired 0:5b4f19f8cd85 12 #define MotorVKp_End 50
heroistired 0:5b4f19f8cd85 13 #define MotorVKi_End 0
heroistired 0:5b4f19f8cd85 14
heroistired 0:5b4f19f8cd85 15 #define MotorKp_Start 1
heroistired 0:5b4f19f8cd85 16 #define MotorKi_Start 0
heroistired 0:5b4f19f8cd85 17 #define MotorVKp_Start 50
heroistired 0:5b4f19f8cd85 18 #define MotorVKi_Start 0
heroistired 0:5b4f19f8cd85 19
heroistired 0:5b4f19f8cd85 20
heroistired 0:5b4f19f8cd85 21 //电机控制相关常量和变量
heroistired 0:5b4f19f8cd85 22 #define APB2Freq 72000000
heroistired 0:5b4f19f8cd85 23 /*#define Motor1Dir PAout(2) //电机方向引脚的宏定义
heroistired 0:5b4f19f8cd85 24 #define Motor2Dir PAout(3)
heroistired 0:5b4f19f8cd85 25 #define Motor3Dir PAout(8)*/
heroistired 0:5b4f19f8cd85 26
heroistired 0:5b4f19f8cd85 27 //电机初始化过程状态机的状态宏定义
heroistired 0:5b4f19f8cd85 28 #define MotorInitState_Out 0 //最开始的状态，默认舵臂在光电开关外面
heroistired 0:5b4f19f8cd85 29 #define MotorInitState_In 1 //首先舵臂需要下降到光电开关里面，处于这个状态时，电机时往光电开关里面走，而不是已经在光电开关里面了，下面同
heroistired 0:5b4f19f8cd85 30 #define MotorInitState_OutAgain 2 //然后需要到光电开关外面去
heroistired 0:5b4f19f8cd85 31 #define MotorInitState_InAgain 3 //然后再到关电开关里面去，这时候要打开外部中断，舵臂的遮挡动作会触发外部中断，此时会记下初始位置，作为零位置
heroistired 0:5b4f19f8cd85 32 #define MotorInitState_BackToZero 4 //初始化成功
heroistired 0:5b4f19f8cd85 33 #define MotorInitState_Succeed 5 //初始化成功
heroistired 0:5b4f19f8cd85 34
heroistired 0:5b4f19f8cd85 35 #define ID_Motor1 1 //电机的ID号码
heroistired 0:5b4f19f8cd85 36 #define ID_Motor2 2
heroistired 0:5b4f19f8cd85 37 #define ID_Motor3 3
heroistired 0:5b4f19f8cd85 38 #define MotorStatus_Running 1 //电机的运行状态
heroistired 0:5b4f19f8cd85 39 #define MotorStatus_OK 2
heroistired 0:5b4f19f8cd85 40 //防抖动算法的变量和常量
heroistired 0:5b4f19f8cd85 41 #define ErrorTolerance 20 //误差容限
heroistired 0:5b4f19f8cd85 42 #define INT_TimesMin 10 //连续INT_TimesMin次中断读数都在误差容限之内，则判定已经到达预定位置
heroistired 0:5b4f19f8cd85 43
heroistired 0:5b4f19f8cd85 44 //电机控制参数变换 需要变参数运行
heroistired 0:5b4f19f8cd85 45 #define ApproachDistance 1000 //接近距离 当轴的实际位置与目标位置接近到一定程度之后，应当把参数调小一些，免得蜜汁抖动
heroistired 0:5b4f19f8cd85 46
heroistired 0:5b4f19f8cd85 47 //低通滤波器相关常量
heroistired 0:5b4f19f8cd85 48 #define FlierCoefficient 0.31415926 //低通滤波器系数 截止频率10Hz
heroistired 0:5b4f19f8cd85 49
heroistired 0:5b4f19f8cd85 50
heroistired 0:5b4f19f8cd85 51 #define PulseFreqLimit 10000
heroistired 0:5b4f19f8cd85 52
heroistired 0:5b4f19f8cd85 53 //电机控制结构体
heroistired 0:5b4f19f8cd85 54 typedef struct _Motor_Type
heroistired 0:5b4f19f8cd85 55 {
heroistired 0:5b4f19f8cd85 56 //PI调节器部分
heroistired 0:5b4f19f8cd85 57 //位置环
heroistired 0:5b4f19f8cd85 58 int Encoder; //编码器读数
heroistired 0:5b4f19f8cd85 59 int EncoderTemp; //编码器数据处理的中间值
heroistired 0:5b4f19f8cd85 60
heroistired 0:5b4f19f8cd85 61 int PIDControl; //PID控制量
heroistired 0:5b4f19f8cd85 62 int PIDControl_pre; //PID控制量
heroistired 0:5b4f19f8cd85 63 int Difference; //误差量
heroistired 0:5b4f19f8cd85 64 int Integral; //积分量
heroistired 0:5b4f19f8cd85 65 float Kp; //比例系数
heroistired 0:5b4f19f8cd85 66 float Ki; //积分系数
heroistired 0:5b4f19f8cd85 67 int Goal; //目标值
heroistired 0:5b4f19f8cd85 68 //速度环
heroistired 0:5b4f19f8cd85 69 int Velocity; //速度
heroistired 0:5b4f19f8cd85 70 int V_PIDControl; //PID控制量
heroistired 0:5b4f19f8cd85 71 int V_PIDControl_pre; //PID控制量
heroistired 0:5b4f19f8cd85 72 int V_Difference; //误差量
heroistired 0:5b4f19f8cd85 73 int V_Integral; //积分量
heroistired 0:5b4f19f8cd85 74 float V_Kp; //比例系数
heroistired 0:5b4f19f8cd85 75 float V_Ki; //积分系数
heroistired 0:5b4f19f8cd85 76 int V_Goal; //目标值
heroistired 0:5b4f19f8cd85 77
heroistired 0:5b4f19f8cd85 78 //编码器过零点
heroistired 0:5b4f19f8cd85 79 int LagFiliterValue; //滞后滤波值 用于编码器过零点检测
heroistired 0:5b4f19f8cd85 80 int LagFiliterValuePre; //缓存值
heroistired 0:5b4f19f8cd85 81 int ChangeRateLagFiliterValue; //变化率
heroistired 0:5b4f19f8cd85 82 char ChangeRateLagFiliterValueDigital; //数字化之后的变化率
heroistired 0:5b4f19f8cd85 83 char ChangeRateLagFiliterValueDigitalPre; //缓存值
heroistired 0:5b4f19f8cd85 84
heroistired 0:5b4f19f8cd85 85 char InZeroZone; //进入零点区域 1 从大侧进入 2 从小侧进入 0 没进入
heroistired 0:5b4f19f8cd85 86 int ZeroZoneCounter; //零点区域的计数器
heroistired 0:5b4f19f8cd85 87
heroistired 0:5b4f19f8cd85 88 int BigStepEncoderValue; //大跨度编码器数据采样值 间隔若干个周期
heroistired 0:5b4f19f8cd85 89 int BigStepEncoderValuePre; //缓存
heroistired 0:5b4f19f8cd85 90 int BigStepCounter; //大跨度采样计数器
heroistired 0:5b4f19f8cd85 91 int BigStepRevolveTimes;
heroistired 0:5b4f19f8cd85 92
heroistired 0:5b4f19f8cd85 93 char HandUp1; //检测到过零点动作1
heroistired 0:5b4f19f8cd85 94 char HandUp2; //检测到过零点动作2
heroistired 0:5b4f19f8cd85 95
heroistired 0:5b4f19f8cd85 96 //int EncoderValueEstimate; //依据给的
heroistired 0:5b4f19f8cd85 97
heroistired 0:5b4f19f8cd85 98 //加入减速器后的处理
heroistired 0:5b4f19f8cd85 99 int RevolveTimes; //旋转的圈数
heroistired 0:5b4f19f8cd85 100 int ZeroPosition; //零点的编码器读数
heroistired 0:5b4f19f8cd85 101 int PresentPosition; //换算后的减速器轴的位置
heroistired 0:5b4f19f8cd85 102 char ZeroPosFound; //是否找到零位置的标志
heroistired 0:5b4f19f8cd85 103 char ZeroPosFoundAll; //电机全部都找到了零位置
heroistired 0:5b4f19f8cd85 104
heroistired 0:5b4f19f8cd85 105 //初始化部分 状态机
heroistired 0:5b4f19f8cd85 106 char Init_State; //记录电机正处于初始化的哪一个状态
heroistired 0:5b4f19f8cd85 107 char Init_ErrorTimes; //电机初始化过程中出错的次数
heroistired 0:5b4f19f8cd85 108
heroistired 0:5b4f19f8cd85 109 //防抖动部分
heroistired 0:5b4f19f8cd85 110 int INTCounter; //记录编码器读数进入误差容限后中断执行的次数
heroistired 0:5b4f19f8cd85 111 int EncoderPre; //前一次编码器的读数值(滤波后)
heroistired 0:5b4f19f8cd85 112 int Difference_pre;
heroistired 0:5b4f19f8cd85 113 //增强电机性能
heroistired 0:5b4f19f8cd85 114 int ChangeRatePIDControl;
heroistired 0:5b4f19f8cd85 115 int ChangeRatePIDControlPre;
heroistired 0:5b4f19f8cd85 116 int ChangeRateDifference;
heroistired 0:5b4f19f8cd85 117 //身份识别
heroistired 0:5b4f19f8cd85 118 char ID; //电机的ID号码 这个程序自动设置的
heroistired 0:5b4f19f8cd85 119 //运行状态
heroistired 0:5b4f19f8cd85 120 char Status; //分为 运行中 和 达到目标值
heroistired 0:5b4f19f8cd85 121 char Dir; //电机的方向
heroistired 0:5b4f19f8cd85 122 char GoalUpdated; //目标值是否已经更新
heroistired 0:5b4f19f8cd85 123 DigitalOut *MotorDir; //控制电机方向的管脚
heroistired 0:5b4f19f8cd85 124 DigitalOut *EncoderCs; //编码器片选脚
heroistired 0:5b4f19f8cd85 125 SPI *SPI_Encoder; //编码器对象
heroistired 0:5b4f19f8cd85 126 PwmOut *MotorPwm; //PWM对象
heroistired 0:5b4f19f8cd85 127 }MotorType;
heroistired 0:5b4f19f8cd85 128
heroistired 0:5b4f19f8cd85 129 void StepMotorInit(MotorType *Motor, DigitalOut *MotorDirPin, DigitalOut *EncoderCsPin, SPI *SPI_EncoderObj);
heroistired 0:5b4f19f8cd85 130
heroistired 0:5b4f19f8cd85 131 void MotorPIDController(MotorType *Motor);
heroistired 0:5b4f19f8cd85 132
heroistired 0:5b4f19f8cd85 133 int Math_abs(int num);
heroistired 0:5b4f19f8cd85 134
heroistired 0:5b4f19f8cd85 135
heroistired 0:5b4f19f8cd85 136 //读取编码器值
heroistired 0:5b4f19f8cd85 137 unsigned int Encoder_ReadData(MotorType *Motor);
heroistired 0:5b4f19f8cd85 138
heroistired 0:5b4f19f8cd85 139 //编码器初始化
heroistired 0:5b4f19f8cd85 140 void Encoder_Init(MotorType *Motor);
heroistired 0:5b4f19f8cd85 141
heroistired 0:5b4f19f8cd85 142 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 143 //功能：一阶低通滤波
heroistired 0:5b4f19f8cd85 144 //参数：Value_now 本次样值 Value_pre 上次滤波输出值
heroistired 0:5b4f19f8cd85 145 //返回值：本次滤波输出值
heroistired 0:5b4f19f8cd85 146 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 147 int Fliter_1(int Value_now, int Value_pre);
heroistired 0:5b4f19f8cd85 148
heroistired 0:5b4f19f8cd85 149 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 150 //功能：设置步进电机脉冲信号的PWM频率
heroistired 0:5b4f19f8cd85 151 //
heroistired 0:5b4f19f8cd85 152 //返回值：无
heroistired 0:5b4f19f8cd85 153 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 154 void StepMotorSetFreq(MotorType *Motor);
heroistired 0:5b4f19f8cd85 155
heroistired 0:5b4f19f8cd85 156 //控制接口
heroistired 0:5b4f19f8cd85 157 void Communication(Serial *Uart, MotorType *Motor);
heroistired 0:5b4f19f8cd85 158
heroistired 0:5b4f19f8cd85 159 #endif