Chen Huan
/
MotorPID
闭环步进电机
Diff: motor.h
- Revision:
- 0:5b4f19f8cd85
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor.h Fri Mar 30 12:03:12 2018 +0000 @@ -0,0 +1,159 @@ +#ifndef __MOTOR_H +#define __MOTOR_H +#include "mbed.h" + +#define MotorKp 1 +#define MotorKi 0 +#define MotorVKp 50 +#define MotorVKi 0 + +#define MotorKp_End 1 +#define MotorKi_End 0 +#define MotorVKp_End 50 +#define MotorVKi_End 0 + +#define MotorKp_Start 1 +#define MotorKi_Start 0 +#define MotorVKp_Start 50 +#define MotorVKi_Start 0 + + +//电机控制相关常量和变量 +#define APB2Freq 72000000 +/*#define Motor1Dir PAout(2) //电机方向引脚的宏定义 +#define Motor2Dir PAout(3) +#define Motor3Dir PAout(8)*/ + +//电机初始化过程状态机的状态宏定义 +#define MotorInitState_Out 0 //最开始的状态,默认舵臂在光电开关外面 +#define MotorInitState_In 1 //首先舵臂需要下降到光电开关里面,处于这个状态时,电机时往光电开关里面走,而不是已经在光电开关里面了,下面同 +#define MotorInitState_OutAgain 2 //然后需要到光电开关外面去 +#define MotorInitState_InAgain 3 //然后再到关电开关里面去,这时候要打开外部中断,舵臂的遮挡动作会触发外部中断,此时会记下初始位置,作为零位置 +#define MotorInitState_BackToZero 4 //初始化成功 +#define MotorInitState_Succeed 5 //初始化成功 + +#define ID_Motor1 1 //电机的ID号码 +#define ID_Motor2 2 +#define ID_Motor3 3 +#define MotorStatus_Running 1 //电机的运行状态 +#define MotorStatus_OK 2 +//防抖动算法的变量和常量 +#define ErrorTolerance 20 //误差容限 +#define INT_TimesMin 10 //连续INT_TimesMin次中断读数都在误差容限之内,则判定已经到达预定位置 + +//电机控制参数变换 需要变参数运行 +#define ApproachDistance 1000 //接近距离 当轴的实际位置与目标位置接近到一定程度之后,应当把参数调小一些,免得蜜汁抖动 + +//低通滤波器相关常量 +#define FlierCoefficient 0.31415926 //低通滤波器系数 截止频率10Hz + + +#define PulseFreqLimit 10000 + +//电机控制结构体 +typedef struct _Motor_Type +{ + //PI调节器部分 + //位置环 + int Encoder; //编码器读数 + int EncoderTemp; //编码器数据处理的中间值 + + int PIDControl; //PID控制量 + int PIDControl_pre; //PID控制量 + int Difference; //误差量 + int Integral; //积分量 + float Kp; //比例系数 + float Ki; //积分系数 + int Goal; //目标值 + //速度环 + int Velocity; //速度 + int V_PIDControl; //PID控制量 + int V_PIDControl_pre; //PID控制量 + int V_Difference; //误差量 + int V_Integral; //积分量 + float V_Kp; //比例系数 + float V_Ki; //积分系数 + int V_Goal; //目标值 + + //编码器过零点 + int LagFiliterValue; //滞后滤波值 用于编码器过零点检测 + int LagFiliterValuePre; //缓存值 + int ChangeRateLagFiliterValue; //变化率 + char ChangeRateLagFiliterValueDigital; //数字化之后的变化率 + char ChangeRateLagFiliterValueDigitalPre; //缓存值 + + char InZeroZone; //进入零点区域 1 从大侧进入 2 从小侧进入 0 没进入 + int ZeroZoneCounter; //零点区域的计数器 + + int BigStepEncoderValue; //大跨度编码器数据采样值 间隔若干个周期 + int BigStepEncoderValuePre; //缓存 + int BigStepCounter; //大跨度采样计数器 + int BigStepRevolveTimes; + + char HandUp1; //检测到过零点动作1 + char HandUp2; //检测到过零点动作2 + + //int EncoderValueEstimate; //依据给的 + + //加入减速器后的处理 + int RevolveTimes; //旋转的圈数 + int ZeroPosition; //零点的编码器读数 + int PresentPosition; //换算后的减速器轴的位置 + char ZeroPosFound; //是否找到零位置的标志 + char ZeroPosFoundAll; //电机全部都找到了零位置 + + //初始化部分 状态机 + char Init_State; //记录电机正处于初始化的哪一个状态 + char Init_ErrorTimes; //电机初始化过程中出错的次数 + + //防抖动部分 + int INTCounter; //记录编码器读数进入误差容限后中断执行的次数 + int EncoderPre; //前一次编码器的读数值(滤波后) + int Difference_pre; + //增强电机性能 + int ChangeRatePIDControl; + int ChangeRatePIDControlPre; + int ChangeRateDifference; + //身份识别 + char ID; //电机的ID号码 这个程序自动设置的 + //运行状态 + char Status; //分为 运行中 和 达到目标值 + char Dir; //电机的方向 + char GoalUpdated; //目标值是否已经更新 + DigitalOut *MotorDir; //控制电机方向的管脚 + DigitalOut *EncoderCs; //编码器片选脚 + SPI *SPI_Encoder; //编码器对象 + PwmOut *MotorPwm; //PWM对象 +}MotorType; + +void StepMotorInit(MotorType *Motor, DigitalOut *MotorDirPin, DigitalOut *EncoderCsPin, SPI *SPI_EncoderObj); + +void MotorPIDController(MotorType *Motor); + +int Math_abs(int num); + + +//读取编码器值 +unsigned int Encoder_ReadData(MotorType *Motor); + +//编码器初始化 +void Encoder_Init(MotorType *Motor); + +////////////////////////////////////////////////////////////////////////////////// +//功能:一阶低通滤波 +//参数:Value_now 本次样值 Value_pre 上次滤波输出值 +//返回值:本次滤波输出值 +////////////////////////////////////////////////////////////////////////////////// +int Fliter_1(int Value_now, int Value_pre); + +////////////////////////////////////////////////////////////////////////////////// +//功能:设置步进电机脉冲信号的PWM频率 +// +//返回值:无 +////////////////////////////////////////////////////////////////////////////////// +void StepMotorSetFreq(MotorType *Motor); + +//控制接口 +void Communication(Serial *Uart, MotorType *Motor); + +#endif \ No newline at end of file