Chen Huan
/
MotorPID
闭环步进电机
Diff: motor.cpp
- Revision:
- 0:5b4f19f8cd85
- Child:
- 1:cbd6a3232d5b
diff -r 000000000000 -r 5b4f19f8cd85 motor.cpp --- /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