Chen Huan
闭环步进电机
Diff: motor.cpp
- Revision:
- 0:5b4f19f8cd85
- Child:
- 1:cbd6a3232d5b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.cpp Fri Mar 30 12:03:12 2018 +0000
@@ -0,0 +1,255 @@
+#include "motor.h"
+
+
+//////////////////////////////////////////////////////////////////////////////////
+//功能：电机结构体初始化
+//参数：Motor：电机控制结构体
+//返回值：无
+//////////////////////////////////////////////////////////////////////////////////
+void StepMotorInit(MotorType *Motor, DigitalOut *MotorDirPin, DigitalOut *EncoderCsPin, SPI *SPI_EncoderObj)
+{
+ RCC->APB1ENR|=1<<0; //TIM2时钟使能
+ RCC->APB2ENR|=1<<2; //GPIOA时钟使能
+ GPIOA->CRL&=0XFFFFFF0F; //PA1清除之前的设置
+ GPIOA->CRL|=0X000000B0; //复用功能输出
+
+ TIM2->ARR=899; //设定计数器自动重装值 定时器时钟频率 72K 固定的
+ TIM2->PSC=999; //预分频器设置
+
+ TIM2->CCMR1|=7<<12; //CH2 PWM2模式
+ TIM2->CCMR1|=1<<11; //CH2预装载使能
+ TIM2->CCER|=1<<4; //OC2 输出使能
+
+ TIM2->CR1=0x0080; //ARPE使能
+ TIM2->CR1|=0x01; //使能定时器2
+
+ //初始胡电机控制结构体
+ Motor->Encoder = 0;
+ Motor->Difference = 0;
+ Motor->Goal = 0;
+ Motor->Integral = 0;
+ Motor->PIDControl = 0;
+ Motor->Kp = (float)MotorKp;
+ Motor->Ki = (float)MotorKi;
+ Motor->INTCounter = 0;
+ Motor->EncoderPre = 0;
+ Motor->ID = ID_Motor1;
+ Motor->Status = MotorStatus_Running;
+ Motor->Dir = 0; //默认使编码器读数减小的方向
+ Motor->ChangeRatePIDControl = 0;
+ Motor->PIDControl_pre = 0;
+ Motor->Velocity = 0;
+ Motor->V_PIDControl = 0;
+ Motor->V_PIDControl_pre = 0;
+ Motor->V_Difference = 0;
+ Motor->V_Integral = 0;
+ Motor->V_Kp = MotorVKp;
+ Motor->V_Ki = MotorVKi;
+ Motor->V_Goal = 0;
+ Motor->GoalUpdated = 0;
+ Motor->RevolveTimes = 0;
+ Motor->ZeroPosition = 0;
+ Motor->PresentPosition = 0;
+ Motor->ZeroPosFound = 0;
+ Motor->ZeroPosFoundAll = 0;
+ Motor->Init_State = MotorInitState_Out;
+ Motor->Init_ErrorTimes = 0;
+ Motor->LagFiliterValue = 0;
+ Motor->LagFiliterValuePre = 0;
+ Motor->ChangeRateLagFiliterValue = 0;
+ Motor->ChangeRateLagFiliterValueDigital = 0;
+ Motor->ChangeRateLagFiliterValueDigitalPre = 0;
+ Motor->InZeroZone = 0;
+ Motor->ZeroZoneCounter = 0;
+ Motor->BigStepCounter = 0;
+ Motor->BigStepEncoderValue = 0;
+ Motor->BigStepEncoderValuePre = 0;
+ Motor->BigStepRevolveTimes = 0;
+ Motor->HandUp1 = 0;
+ Motor->HandUp2 = 0;
+ Motor->EncoderTemp = 0;
+ Motor->ChangeRatePIDControlPre = 0;
+ Motor->MotorDir = MotorDirPin;
+ Motor->EncoderCs = EncoderCsPin;
+ Motor->SPI_Encoder = SPI_EncoderObj;
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+//功能：电机控制的PI调节器
+//参数：Motor：要计算PID控制量的电机对应的电机控制结构体
+//返回值：无
+//////////////////////////////////////////////////////////////////////////////////
+void MotorPIDController(MotorType *Motor)
+{
+ char ChangeRateDifferenceLimit = 50;
+
+ //位置偏差滤波 限制上升率 和 限制幅值
+ Motor->Difference_pre = Motor->Difference;
+ Motor->Difference = -(Motor->Encoder- Motor->Goal); //计算偏差
+ Motor->ChangeRateDifference = Motor->Difference - Motor->Difference_pre;
+
+ //切换参数
+ if(Math_abs(Motor->Difference) <= ApproachDistance) //很接近目标距离了
+ {
+ Motor->Ki = MotorKi_End;
+ Motor->Kp = MotorKp_End;
+ Motor->V_Ki = MotorVKi_End;
+ Motor->V_Kp = MotorVKp_End;
+ }
+ else if(Motor->Velocity <= 15)
+ {
+ Motor->Ki = MotorKi_Start;
+ Motor->Kp = MotorKp_Start;
+ Motor->V_Ki = MotorVKi_Start;
+ Motor->V_Kp = MotorVKp_Start;
+ }
+ else
+ {
+ Motor->Ki = MotorKi;
+ Motor->Kp = MotorKp;
+ Motor->V_Ki = MotorVKi;
+ Motor->V_Kp = MotorVKp;
+ }
+
+ Motor->Integral += Motor->Difference; //计算积分量
+ if(Motor->Integral>15000) Motor->Integral=15000; //积分限幅
+ if(Motor->Integral<-15000) Motor->Integral=-15000;
+
+ //速度目标值
+ Motor->V_Goal = (Motor->Kp * Motor->Difference + Motor->Ki * Motor->Integral)/10; //速度环目标值
+ Motor->GoalUpdated = 0;
+
+
+ Motor->Velocity = Motor->Encoder - Motor->EncoderPre;
+ Motor->V_Difference = -(Motor->Velocity - Motor->V_Goal); //速度偏差
+ Motor->V_Integral += Motor->V_Difference;
+ if(Motor->V_Integral>6000) Motor->V_Integral=6000; //积分限幅
+ if(Motor->V_Integral<-6000) Motor->V_Integral=-6000;
+ Motor->PIDControl = (Motor->V_Kp * Motor->V_Difference + Motor->V_Ki * Motor->V_Integral); //计算控制量以及相应的数据处理
+
+
+ if(Motor->PIDControl < 0)
+ {
+ Motor->PIDControl = -Motor->PIDControl;
+ Motor->Dir = 0;
+ Motor->MotorDir->write(0);
+ }
+ else
+ {
+ Motor->Dir = 1;
+ Motor->MotorDir->write(1);
+ }
+
+ //判断是否进入误差容限
+ if(Math_abs(Motor->Difference) <= ErrorTolerance)
+ {
+ if(Motor->INTCounter <= INT_TimesMin)
+ Motor->INTCounter++;
+ else
+ {
+ Motor->PIDControl = 0;
+ Motor->Status = MotorStatus_OK;
+ }
+ }
+ else
+ {
+ Motor->INTCounter = 0;
+ Motor->Status = MotorStatus_Running;
+ }
+
+ //限幅
+ if(Motor->PIDControl > PulseFreqLimit)
+ Motor->PIDControl = PulseFreqLimit;
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+//功能：求绝对值
+//参数：num 带求绝对值的函数
+//返回值：num的绝对值
+//////////////////////////////////////////////////////////////////////////////////
+int Math_abs(int num)
+{
+ if(num < 0)
+ return -num;
+ else
+ return num;
+}
+
+//编码器初始化
+void Encoder_Init(MotorType *Motor)
+{
+ Motor->EncoderCs->write(1);
+ Motor->SPI_Encoder->format(16,1);
+ Motor->SPI_Encoder->frequency(100000);
+}
+
+//读取编码器值
+unsigned int Encoder_ReadData(MotorType *Motor)
+{
+ Motor->EncoderCs->write(0);
+ int response = Motor->SPI_Encoder->write(0xFFFF);
+ response &= 0x3fff;
+ Motor->EncoderCs->write(1);
+ return response;
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+//功能：一阶低通滤波
+//参数：Value_now 本次样值 Value_pre 上次滤波输出值
+//返回值：本次滤波输出值
+//////////////////////////////////////////////////////////////////////////////////
+int Fliter_1(int Value_now, int Value_pre)
+{
+ return (int)(FlierCoefficient*(float)Value_now+(1-FlierCoefficient)*(float)Value_pre);
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+//功能：设置步进电机脉冲信号的PWM频率
+//
+//返回值：无
+//////////////////////////////////////////////////////////////////////////////////
+void StepMotorSetFreq(MotorType *Motor)
+{
+ unsigned int Motor_ARR;
+ //为了使频率步长1Hz平滑可调需要分段设置定时器的时钟频率，即满足(Tclk/freq-Tclk/freq)>1
+ //并且考虑到余量与ARR寄存器只有16位，设置分段如下
+ //8k-5k 72M
+ //5k-1k 36M
+ //1k-500 2M
+ //500-100 720k
+ //100-1 72k
+ //Motor->PIDControl = 5000;
+ if(Motor->PIDControl > PulseFreqLimit)
+ Motor->PIDControl = PulseFreqLimit;
+ else if((Motor->PIDControl <= PulseFreqLimit) && (Motor->PIDControl > 5000))
+ TIM2->PSC=0; //预分频器设置
+ else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 1000))
+ TIM2->PSC=1; //预分频器设置
+ else if((Motor->PIDControl <= 1000) && (Motor->PIDControl > 500))
+ TIM2->PSC=35; //预分频器设置
+ else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 100))
+ TIM2->PSC=99; //预分频器设置
+ else if((Motor->PIDControl <= 100) && (Motor->PIDControl > 0))
+ TIM2->PSC=999; //预分频器设置
+ else
+ {
+ Motor->PIDControl = 1;
+ TIM2->PSC=999; //预分频器设置
+ }
+
+ Motor_ARR = APB2Freq / (TIM2->PSC+1) / Motor->PIDControl;
+ //printf("ll%dll \r\n", Motor_ARR);
+ TIM2->ARR = Motor_ARR;
+ TIM2->CCR2 = Motor_ARR/2;
+}
+
+//控制接口
+void Communication(Serial *Uart, MotorType *Motor)
+{
+ char UartTemp = 0; //串口缓存
+ if(Uart->readable())
+ {
+
+ }
+}
\ No newline at end of file