Chen Huan / Mbed 2 deprecated MotorPID

闭环步进电机

Dependencies:   mbed

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
heroistired 0:5b4f19f8cd85 1 #include "motor.h"
heroistired 0:5b4f19f8cd85 2
heroistired 0:5b4f19f8cd85 3
heroistired 0:5b4f19f8cd85 4 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 5 //功能：电机结构体初始化
heroistired 0:5b4f19f8cd85 6 //参数：Motor：电机控制结构体
heroistired 0:5b4f19f8cd85 7 //返回值：无
heroistired 0:5b4f19f8cd85 8 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 9 void StepMotorInit(MotorType *Motor, DigitalOut *MotorDirPin, DigitalOut *EncoderCsPin, SPI *SPI_EncoderObj)
heroistired 0:5b4f19f8cd85 10 {
heroistired 0:5b4f19f8cd85 11 RCC->APB1ENR|=1<<0; //TIM2时钟使能
heroistired 0:5b4f19f8cd85 12 RCC->APB2ENR|=1<<2; //GPIOA时钟使能
heroistired 0:5b4f19f8cd85 13 GPIOA->CRL&=0XFFFFFF0F; //PA1清除之前的设置
heroistired 0:5b4f19f8cd85 14 GPIOA->CRL|=0X000000B0; //复用功能输出
heroistired 0:5b4f19f8cd85 15
heroistired 0:5b4f19f8cd85 16 TIM2->ARR=899; //设定计数器自动重装值 定时器时钟频率 72K 固定的
heroistired 0:5b4f19f8cd85 17 TIM2->PSC=999; //预分频器设置
heroistired 0:5b4f19f8cd85 18
heroistired 0:5b4f19f8cd85 19 TIM2->CCMR1|=7<<12; //CH2 PWM2模式
heroistired 0:5b4f19f8cd85 20 TIM2->CCMR1|=1<<11; //CH2预装载使能
heroistired 0:5b4f19f8cd85 21 TIM2->CCER|=1<<4; //OC2 输出使能
heroistired 0:5b4f19f8cd85 22
heroistired 0:5b4f19f8cd85 23 TIM2->CR1=0x0080; //ARPE使能
heroistired 0:5b4f19f8cd85 24 TIM2->CR1|=0x01; //使能定时器2
heroistired 0:5b4f19f8cd85 25
heroistired 0:5b4f19f8cd85 26 //初始胡电机控制结构体
heroistired 0:5b4f19f8cd85 27 Motor->Encoder = 0;
heroistired 0:5b4f19f8cd85 28 Motor->Difference = 0;
heroistired 0:5b4f19f8cd85 29 Motor->Goal = 0;
heroistired 0:5b4f19f8cd85 30 Motor->Integral = 0;
heroistired 0:5b4f19f8cd85 31 Motor->PIDControl = 0;
heroistired 0:5b4f19f8cd85 32 Motor->Kp = (float)MotorKp;
heroistired 0:5b4f19f8cd85 33 Motor->Ki = (float)MotorKi;
heroistired 0:5b4f19f8cd85 34 Motor->INTCounter = 0;
heroistired 0:5b4f19f8cd85 35 Motor->EncoderPre = 0;
heroistired 0:5b4f19f8cd85 36 Motor->ID = ID_Motor1;
heroistired 0:5b4f19f8cd85 37 Motor->Status = MotorStatus_Running;
heroistired 0:5b4f19f8cd85 38 Motor->Dir = 0; //默认使编码器读数减小的方向
heroistired 0:5b4f19f8cd85 39 Motor->ChangeRatePIDControl = 0;
heroistired 0:5b4f19f8cd85 40 Motor->PIDControl_pre = 0;
heroistired 0:5b4f19f8cd85 41 Motor->Velocity = 0;
heroistired 0:5b4f19f8cd85 42 Motor->V_PIDControl = 0;
heroistired 0:5b4f19f8cd85 43 Motor->V_PIDControl_pre = 0;
heroistired 0:5b4f19f8cd85 44 Motor->V_Difference = 0;
heroistired 0:5b4f19f8cd85 45 Motor->V_Integral = 0;
heroistired 0:5b4f19f8cd85 46 Motor->V_Kp = MotorVKp;
heroistired 0:5b4f19f8cd85 47 Motor->V_Ki = MotorVKi;
heroistired 0:5b4f19f8cd85 48 Motor->V_Goal = 0;
heroistired 0:5b4f19f8cd85 49 Motor->GoalUpdated = 0;
heroistired 0:5b4f19f8cd85 50 Motor->RevolveTimes = 0;
heroistired 0:5b4f19f8cd85 51 Motor->ZeroPosition = 0;
heroistired 0:5b4f19f8cd85 52 Motor->PresentPosition = 0;
heroistired 0:5b4f19f8cd85 53 Motor->ZeroPosFound = 0;
heroistired 0:5b4f19f8cd85 54 Motor->ZeroPosFoundAll = 0;
heroistired 0:5b4f19f8cd85 55 Motor->Init_State = MotorInitState_Out;
heroistired 0:5b4f19f8cd85 56 Motor->Init_ErrorTimes = 0;
heroistired 0:5b4f19f8cd85 57 Motor->LagFiliterValue = 0;
heroistired 0:5b4f19f8cd85 58 Motor->LagFiliterValuePre = 0;
heroistired 0:5b4f19f8cd85 59 Motor->ChangeRateLagFiliterValue = 0;
heroistired 0:5b4f19f8cd85 60 Motor->ChangeRateLagFiliterValueDigital = 0;
heroistired 0:5b4f19f8cd85 61 Motor->ChangeRateLagFiliterValueDigitalPre = 0;
heroistired 0:5b4f19f8cd85 62 Motor->InZeroZone = 0;
heroistired 0:5b4f19f8cd85 63 Motor->ZeroZoneCounter = 0;
heroistired 0:5b4f19f8cd85 64 Motor->BigStepCounter = 0;
heroistired 0:5b4f19f8cd85 65 Motor->BigStepEncoderValue = 0;
heroistired 0:5b4f19f8cd85 66 Motor->BigStepEncoderValuePre = 0;
heroistired 0:5b4f19f8cd85 67 Motor->BigStepRevolveTimes = 0;
heroistired 0:5b4f19f8cd85 68 Motor->HandUp1 = 0;
heroistired 0:5b4f19f8cd85 69 Motor->HandUp2 = 0;
heroistired 0:5b4f19f8cd85 70 Motor->EncoderTemp = 0;
heroistired 0:5b4f19f8cd85 71 Motor->ChangeRatePIDControlPre = 0;
heroistired 0:5b4f19f8cd85 72 Motor->MotorDir = MotorDirPin;
heroistired 0:5b4f19f8cd85 73 Motor->EncoderCs = EncoderCsPin;
heroistired 0:5b4f19f8cd85 74 Motor->SPI_Encoder = SPI_EncoderObj;
heroistired 0:5b4f19f8cd85 75 }
heroistired 0:5b4f19f8cd85 76
heroistired 0:5b4f19f8cd85 77 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 78 //功能：电机控制的PI调节器
heroistired 0:5b4f19f8cd85 79 //参数：Motor：要计算PID控制量的电机对应的电机控制结构体
heroistired 0:5b4f19f8cd85 80 //返回值：无
heroistired 0:5b4f19f8cd85 81 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 82 void MotorPIDController(MotorType *Motor)
heroistired 0:5b4f19f8cd85 83 {
heroistired 0:5b4f19f8cd85 84 char ChangeRateDifferenceLimit = 50;
heroistired 0:5b4f19f8cd85 85
heroistired 0:5b4f19f8cd85 86 //位置偏差滤波 限制上升率 和 限制幅值
heroistired 0:5b4f19f8cd85 87 Motor->Difference_pre = Motor->Difference;
heroistired 0:5b4f19f8cd85 88 Motor->Difference = -(Motor->Encoder- Motor->Goal); //计算偏差
heroistired 0:5b4f19f8cd85 89 Motor->ChangeRateDifference = Motor->Difference - Motor->Difference_pre;
heroistired 0:5b4f19f8cd85 90
heroistired 0:5b4f19f8cd85 91 //切换参数
heroistired 0:5b4f19f8cd85 92 if(Math_abs(Motor->Difference) <= ApproachDistance) //很接近目标距离了
heroistired 0:5b4f19f8cd85 93 {
heroistired 0:5b4f19f8cd85 94 Motor->Ki = MotorKi_End;
heroistired 0:5b4f19f8cd85 95 Motor->Kp = MotorKp_End;
heroistired 0:5b4f19f8cd85 96 Motor->V_Ki = MotorVKi_End;
heroistired 0:5b4f19f8cd85 97 Motor->V_Kp = MotorVKp_End;
heroistired 0:5b4f19f8cd85 98 }
heroistired 0:5b4f19f8cd85 99 else if(Motor->Velocity <= 15)
heroistired 0:5b4f19f8cd85 100 {
heroistired 0:5b4f19f8cd85 101 Motor->Ki = MotorKi_Start;
heroistired 0:5b4f19f8cd85 102 Motor->Kp = MotorKp_Start;
heroistired 0:5b4f19f8cd85 103 Motor->V_Ki = MotorVKi_Start;
heroistired 0:5b4f19f8cd85 104 Motor->V_Kp = MotorVKp_Start;
heroistired 0:5b4f19f8cd85 105 }
heroistired 0:5b4f19f8cd85 106 else
heroistired 0:5b4f19f8cd85 107 {
heroistired 0:5b4f19f8cd85 108 Motor->Ki = MotorKi;
heroistired 0:5b4f19f8cd85 109 Motor->Kp = MotorKp;
heroistired 0:5b4f19f8cd85 110 Motor->V_Ki = MotorVKi;
heroistired 0:5b4f19f8cd85 111 Motor->V_Kp = MotorVKp;
heroistired 0:5b4f19f8cd85 112 }
heroistired 0:5b4f19f8cd85 113
heroistired 0:5b4f19f8cd85 114 Motor->Integral += Motor->Difference; //计算积分量
heroistired 0:5b4f19f8cd85 115 if(Motor->Integral>15000) Motor->Integral=15000; //积分限幅
heroistired 0:5b4f19f8cd85 116 if(Motor->Integral<-15000) Motor->Integral=-15000;
heroistired 0:5b4f19f8cd85 117
heroistired 0:5b4f19f8cd85 118 //速度目标值
heroistired 0:5b4f19f8cd85 119 Motor->V_Goal = (Motor->Kp * Motor->Difference + Motor->Ki * Motor->Integral)/10; //速度环目标值
heroistired 0:5b4f19f8cd85 120 Motor->GoalUpdated = 0;
heroistired 0:5b4f19f8cd85 121
heroistired 0:5b4f19f8cd85 122
heroistired 0:5b4f19f8cd85 123 Motor->Velocity = Motor->Encoder - Motor->EncoderPre;
heroistired 0:5b4f19f8cd85 124 Motor->V_Difference = -(Motor->Velocity - Motor->V_Goal); //速度偏差
heroistired 0:5b4f19f8cd85 125 Motor->V_Integral += Motor->V_Difference;
heroistired 0:5b4f19f8cd85 126 if(Motor->V_Integral>6000) Motor->V_Integral=6000; //积分限幅
heroistired 0:5b4f19f8cd85 127 if(Motor->V_Integral<-6000) Motor->V_Integral=-6000;
heroistired 0:5b4f19f8cd85 128 Motor->PIDControl = (Motor->V_Kp * Motor->V_Difference + Motor->V_Ki * Motor->V_Integral); //计算控制量以及相应的数据处理
heroistired 0:5b4f19f8cd85 129
heroistired 0:5b4f19f8cd85 130
heroistired 0:5b4f19f8cd85 131 if(Motor->PIDControl < 0)
heroistired 0:5b4f19f8cd85 132 {
heroistired 0:5b4f19f8cd85 133 Motor->PIDControl = -Motor->PIDControl;
heroistired 0:5b4f19f8cd85 134 Motor->Dir = 0;
heroistired 0:5b4f19f8cd85 135 Motor->MotorDir->write(0);
heroistired 0:5b4f19f8cd85 136 }
heroistired 0:5b4f19f8cd85 137 else
heroistired 0:5b4f19f8cd85 138 {
heroistired 0:5b4f19f8cd85 139 Motor->Dir = 1;
heroistired 0:5b4f19f8cd85 140 Motor->MotorDir->write(1);
heroistired 0:5b4f19f8cd85 141 }
heroistired 0:5b4f19f8cd85 142
heroistired 0:5b4f19f8cd85 143 //判断是否进入误差容限
heroistired 0:5b4f19f8cd85 144 if(Math_abs(Motor->Difference) <= ErrorTolerance)
heroistired 0:5b4f19f8cd85 145 {
heroistired 0:5b4f19f8cd85 146 if(Motor->INTCounter <= INT_TimesMin)
heroistired 0:5b4f19f8cd85 147 Motor->INTCounter++;
heroistired 0:5b4f19f8cd85 148 else
heroistired 0:5b4f19f8cd85 149 {
heroistired 0:5b4f19f8cd85 150 Motor->PIDControl = 0;
heroistired 0:5b4f19f8cd85 151 Motor->Status = MotorStatus_OK;
heroistired 0:5b4f19f8cd85 152 }
heroistired 0:5b4f19f8cd85 153 }
heroistired 0:5b4f19f8cd85 154 else
heroistired 0:5b4f19f8cd85 155 {
heroistired 0:5b4f19f8cd85 156 Motor->INTCounter = 0;
heroistired 0:5b4f19f8cd85 157 Motor->Status = MotorStatus_Running;
heroistired 0:5b4f19f8cd85 158 }
heroistired 0:5b4f19f8cd85 159
heroistired 0:5b4f19f8cd85 160 //限幅
heroistired 0:5b4f19f8cd85 161 if(Motor->PIDControl > PulseFreqLimit)
heroistired 0:5b4f19f8cd85 162 Motor->PIDControl = PulseFreqLimit;
heroistired 0:5b4f19f8cd85 163
heroistired 0:5b4f19f8cd85 164 }
heroistired 0:5b4f19f8cd85 165
heroistired 0:5b4f19f8cd85 166 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 167 //功能：求绝对值
heroistired 0:5b4f19f8cd85 168 //参数：num 带求绝对值的函数
heroistired 0:5b4f19f8cd85 169 //返回值：num的绝对值
heroistired 0:5b4f19f8cd85 170 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 171 int Math_abs(int num)
heroistired 0:5b4f19f8cd85 172 {
heroistired 0:5b4f19f8cd85 173 if(num < 0)
heroistired 0:5b4f19f8cd85 174 return -num;
heroistired 0:5b4f19f8cd85 175 else
heroistired 0:5b4f19f8cd85 176 return num;
heroistired 0:5b4f19f8cd85 177 }
heroistired 0:5b4f19f8cd85 178
heroistired 0:5b4f19f8cd85 179 //编码器初始化
heroistired 0:5b4f19f8cd85 180 void Encoder_Init(MotorType *Motor)
heroistired 0:5b4f19f8cd85 181 {
heroistired 0:5b4f19f8cd85 182 Motor->EncoderCs->write(1);
heroistired 0:5b4f19f8cd85 183 Motor->SPI_Encoder->format(16,1);
heroistired 0:5b4f19f8cd85 184 Motor->SPI_Encoder->frequency(100000);
heroistired 0:5b4f19f8cd85 185 }
heroistired 0:5b4f19f8cd85 186
heroistired 0:5b4f19f8cd85 187 //读取编码器值
heroistired 0:5b4f19f8cd85 188 unsigned int Encoder_ReadData(MotorType *Motor)
heroistired 0:5b4f19f8cd85 189 {
heroistired 0:5b4f19f8cd85 190 Motor->EncoderCs->write(0);
heroistired 0:5b4f19f8cd85 191 int response = Motor->SPI_Encoder->write(0xFFFF);
heroistired 0:5b4f19f8cd85 192 response &= 0x3fff;
heroistired 0:5b4f19f8cd85 193 Motor->EncoderCs->write(1);
heroistired 0:5b4f19f8cd85 194 return response;
heroistired 0:5b4f19f8cd85 195 }
heroistired 0:5b4f19f8cd85 196
heroistired 0:5b4f19f8cd85 197 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 198 //功能：一阶低通滤波
heroistired 0:5b4f19f8cd85 199 //参数：Value_now 本次样值 Value_pre 上次滤波输出值
heroistired 0:5b4f19f8cd85 200 //返回值：本次滤波输出值
heroistired 0:5b4f19f8cd85 201 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 202 int Fliter_1(int Value_now, int Value_pre)
heroistired 0:5b4f19f8cd85 203 {
heroistired 0:5b4f19f8cd85 204 return (int)(FlierCoefficient*(float)Value_now+(1-FlierCoefficient)*(float)Value_pre);
heroistired 0:5b4f19f8cd85 205 }
heroistired 0:5b4f19f8cd85 206
heroistired 0:5b4f19f8cd85 207 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 208 //功能：设置步进电机脉冲信号的PWM频率
heroistired 0:5b4f19f8cd85 209 //
heroistired 0:5b4f19f8cd85 210 //返回值：无
heroistired 0:5b4f19f8cd85 211 //////////////////////////////////////////////////////////////////////////////////
heroistired 0:5b4f19f8cd85 212 void StepMotorSetFreq(MotorType *Motor)
heroistired 0:5b4f19f8cd85 213 {
heroistired 0:5b4f19f8cd85 214 unsigned int Motor_ARR;
heroistired 0:5b4f19f8cd85 215 //为了使频率步长1Hz平滑可调需要分段设置定时器的时钟频率，即满足(Tclk/freq-Tclk/freq)>1
heroistired 0:5b4f19f8cd85 216 //并且考虑到余量与ARR寄存器只有16位，设置分段如下
heroistired 0:5b4f19f8cd85 217 //8k-5k 72M
heroistired 0:5b4f19f8cd85 218 //5k-1k 36M
heroistired 0:5b4f19f8cd85 219 //1k-500 2M
heroistired 0:5b4f19f8cd85 220 //500-100 720k
heroistired 0:5b4f19f8cd85 221 //100-1 72k
heroistired 0:5b4f19f8cd85 222 //Motor->PIDControl = 5000;
heroistired 0:5b4f19f8cd85 223 if(Motor->PIDControl > PulseFreqLimit)
heroistired 0:5b4f19f8cd85 224 Motor->PIDControl = PulseFreqLimit;
heroistired 0:5b4f19f8cd85 225 else if((Motor->PIDControl <= PulseFreqLimit) && (Motor->PIDControl > 5000))
heroistired 0:5b4f19f8cd85 226 TIM2->PSC=0; //预分频器设置
heroistired 0:5b4f19f8cd85 227 else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 1000))
heroistired 0:5b4f19f8cd85 228 TIM2->PSC=1; //预分频器设置
heroistired 0:5b4f19f8cd85 229 else if((Motor->PIDControl <= 1000) && (Motor->PIDControl > 500))
heroistired 0:5b4f19f8cd85 230 TIM2->PSC=35; //预分频器设置
heroistired 0:5b4f19f8cd85 231 else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 100))
heroistired 0:5b4f19f8cd85 232 TIM2->PSC=99; //预分频器设置
heroistired 0:5b4f19f8cd85 233 else if((Motor->PIDControl <= 100) && (Motor->PIDControl > 0))
heroistired 0:5b4f19f8cd85 234 TIM2->PSC=999; //预分频器设置
heroistired 0:5b4f19f8cd85 235 else
heroistired 0:5b4f19f8cd85 236 {
heroistired 0:5b4f19f8cd85 237 Motor->PIDControl = 1;
heroistired 0:5b4f19f8cd85 238 TIM2->PSC=999; //预分频器设置
heroistired 0:5b4f19f8cd85 239 }
heroistired 0:5b4f19f8cd85 240
heroistired 0:5b4f19f8cd85 241 Motor_ARR = APB2Freq / (TIM2->PSC+1) / Motor->PIDControl;
heroistired 0:5b4f19f8cd85 242 //printf("ll%dll \r\n", Motor_ARR);
heroistired 0:5b4f19f8cd85 243 TIM2->ARR = Motor_ARR;
heroistired 0:5b4f19f8cd85 244 TIM2->CCR2 = Motor_ARR/2;
heroistired 0:5b4f19f8cd85 245 }
heroistired 0:5b4f19f8cd85 246
heroistired 0:5b4f19f8cd85 247 //控制接口
heroistired 0:5b4f19f8cd85 248 void Communication(Serial *Uart, MotorType *Motor)
heroistired 0:5b4f19f8cd85 249 {
heroistired 0:5b4f19f8cd85 250 char UartTemp = 0; //串口缓存
heroistired 0:5b4f19f8cd85 251 if(Uart->readable())
heroistired 0:5b4f19f8cd85 252 {
heroistired 0:5b4f19f8cd85 253
heroistired 0:5b4f19f8cd85 254 }
heroistired 0:5b4f19f8cd85 255 }