Chen Huan
/
MotorPID
闭环步进电机
motor.h@1:cbd6a3232d5b, 2018-04-05 (annotated)
- Committer:
- heroistired
- Date:
- Thu Apr 05 01:37:17 2018 +0000
- Revision:
- 1:cbd6a3232d5b
- Parent:
- 0:5b4f19f8cd85
New
Who changed what in which revision?
User | Revision | Line number | New 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 |