Chen Huan
闭环步进电机
Embed:
(wiki syntax)
Show/hide line numbers
motor.cpp
00001 #include "motor.h" 00002 00003 00004 ////////////////////////////////////////////////////////////////////////////////// 00005 //功能：电机结构体初始化 00006 //参数：Motor：电机控制结构体 00007 //返回值：无 00008 ////////////////////////////////////////////////////////////////////////////////// 00009 void StepMotorInit(MotorType *Motor, DigitalOut *MotorDirPin, DigitalOut *EncoderCsPin, SPI *SPI_EncoderObj) 00010 { 00011 /*RCC->APB1ENR|=1<<0; //TIM2时钟使能 00012 RCC->APB2ENR|=1<<2; //GPIOA时钟使能 00013 GPIOA->CRL&=0XFFFFFF0F; //PA1清除之前的设置 00014 GPIOA->CRL|=0X000000B0; //复用功能输出 00015 00016 TIM2->ARR=899; //设定计数器自动重装值 定时器时钟频率 72K 固定的 00017 TIM2->PSC=999; //预分频器设置 00018 00019 TIM2->CCMR1|=7<<12; //CH2 PWM2模式 00020 TIM2->CCMR1|=1<<11; //CH2预装载使能 00021 TIM2->CCER|=1<<4; //OC2 输出使能 00022 00023 TIM2->CR1=0x0080; //ARPE使能 00024 TIM2->CR1|=0x01; //使能定时器2*/ 00025 00026 RCC->APB2ENR|=1<<11; //TIM1时钟使能 00027 RCC->APB2ENR|=1<<2; //GPIOA时钟使能 00028 GPIOA->CRH&=0XFFFFFFF0; //PA8清除之前的设置 00029 GPIOA->CRH|=0X0000000B; //复用功能输出 00030 00031 TIM1->ARR=899; //设定计数器自动重装值 00032 TIM1->PSC=999; //预分频器设置 00033 00034 TIM1->CCMR1|=7<<4; //CH1 PWM2模式 00035 TIM1->CCMR1|=1<<3; //CH1预装载使能 00036 TIM1->CCER|=1<<0; //OC1 输出使能 00037 00038 TIM1->BDTR|=1<<15; //MOE 主输出使能 00039 00040 TIM1->CR1=0x0080; //ARPE使能 00041 TIM1->CR1|=0x01; //使能定时器1 00042 00043 //初始胡电机控制结构体 00044 Motor->Encoder = 0; 00045 Motor->Difference = 0; 00046 Motor->Goal = 0; 00047 Motor->Integral = 0; 00048 Motor->PIDControl = 0; 00049 Motor->Kp = (float)MotorKp; 00050 Motor->Ki = (float)MotorKi; 00051 Motor->INTCounter = 0; 00052 Motor->EncoderPre = 0; 00053 Motor->ID = ID_Motor1; 00054 Motor->Status = MotorStatus_Running; 00055 Motor->Dir = 0; //默认使编码器读数减小的方向 00056 Motor->ChangeRatePIDControl = 0; 00057 Motor->PIDControl_pre = 0; 00058 Motor->Velocity = 0; 00059 Motor->V_PIDControl = 0; 00060 Motor->V_PIDControl_pre = 0; 00061 Motor->V_Difference = 0; 00062 Motor->V_Integral = 0; 00063 Motor->V_Kp = MotorVKp; 00064 Motor->V_Ki = MotorVKi; 00065 Motor->V_Goal = 0; 00066 Motor->GoalUpdated = 0; 00067 Motor->RevolveTimes = 0; 00068 Motor->ZeroPosition = 0; 00069 Motor->PresentPosition = 0; 00070 Motor->ZeroPosFound = 0; 00071 Motor->ZeroPosFoundAll = 0; 00072 Motor->Init_State = MotorInitState_Out; 00073 Motor->Init_ErrorTimes = 0; 00074 Motor->LagFiliterValue = 0; 00075 Motor->LagFiliterValuePre = 0; 00076 Motor->ChangeRateLagFiliterValue = 0; 00077 Motor->ChangeRateLagFiliterValueDigital = 0; 00078 Motor->ChangeRateLagFiliterValueDigitalPre = 0; 00079 Motor->InZeroZone = 0; 00080 Motor->ZeroZoneCounter = 0; 00081 Motor->BigStepCounter = 0; 00082 Motor->BigStepEncoderValue = 0; 00083 Motor->BigStepEncoderValuePre = 0; 00084 Motor->BigStepRevolveTimes = 0; 00085 Motor->HandUp1 = 0; 00086 Motor->HandUp2 = 0; 00087 Motor->EncoderTemp = 0; 00088 Motor->ChangeRatePIDControlPre = 0; 00089 Motor->MotorDir = MotorDirPin; 00090 Motor->EncoderCs = EncoderCsPin; 00091 Motor->SPI_Encoder = SPI_EncoderObj; 00092 } 00093 00094 ////////////////////////////////////////////////////////////////////////////////// 00095 //功能：电机控制的PI调节器 00096 //参数：Motor：要计算PID控制量的电机对应的电机控制结构体 00097 //返回值：无 00098 ////////////////////////////////////////////////////////////////////////////////// 00099 void MotorPIDController(MotorType *Motor) 00100 { 00101 char ChangeRateDifferenceLimit = 50; 00102 00103 //位置偏差滤波 限制上升率 和 限制幅值 00104 Motor->Difference_pre = Motor->Difference; 00105 Motor->Difference = -(Motor->Encoder- Motor->Goal); //计算偏差 00106 Motor->ChangeRateDifference = Motor->Difference - Motor->Difference_pre; 00107 00108 //切换参数 00109 /*if(Math_abs(Motor->Difference) <= ApproachDistance) //很接近目标距离了 00110 { 00111 Motor->Ki = MotorKi_End; 00112 Motor->Kp = MotorKp_End; 00113 Motor->V_Ki = MotorVKi_End; 00114 Motor->V_Kp = MotorVKp_End; 00115 } 00116 else if(Motor->Velocity <= 15) 00117 { 00118 Motor->Ki = MotorKi_Start; 00119 Motor->Kp = MotorKp_Start; 00120 Motor->V_Ki = MotorVKi_Start; 00121 Motor->V_Kp = MotorVKp_Start; 00122 } 00123 else 00124 { 00125 Motor->Ki = MotorKi; 00126 Motor->Kp = MotorKp; 00127 Motor->V_Ki = MotorVKi; 00128 Motor->V_Kp = MotorVKp; 00129 }*/ 00130 00131 Motor->Integral += Motor->Difference; //计算积分量 00132 if(Motor->Integral>15000) Motor->Integral=15000; //积分限幅 00133 if(Motor->Integral<-15000) Motor->Integral=-15000; 00134 00135 //速度目标值 00136 Motor->V_Goal = (Motor->Kp * Motor->Difference + Motor->Ki * Motor->Integral)/10; //速度环目标值 00137 Motor->GoalUpdated = 0; 00138 00139 00140 Motor->Velocity = Motor->Encoder - Motor->EncoderPre; 00141 Motor->V_Difference = -(Motor->Velocity - Motor->V_Goal); //速度偏差 00142 Motor->V_Integral += Motor->V_Difference; 00143 if(Motor->V_Integral>6000) Motor->V_Integral=6000; //积分限幅 00144 if(Motor->V_Integral<-6000) Motor->V_Integral=-6000; 00145 Motor->PIDControl = (Motor->V_Kp * Motor->V_Difference + Motor->V_Ki * Motor->V_Integral); //计算控制量以及相应的数据处理 00146 00147 00148 if(Motor->PIDControl < 0) 00149 { 00150 Motor->PIDControl = -Motor->PIDControl; 00151 Motor->Dir = 0; 00152 Motor->MotorDir->write(0); 00153 } 00154 else 00155 { 00156 Motor->Dir = 1; 00157 Motor->MotorDir->write(1); 00158 } 00159 00160 //判断是否进入误差容限 00161 if(Math_abs(Motor->Difference) <= ErrorTolerance) 00162 { 00163 if(Motor->INTCounter <= INT_TimesMin) 00164 Motor->INTCounter++; 00165 else 00166 { 00167 Motor->PIDControl = 0; 00168 Motor->Status = MotorStatus_OK; 00169 } 00170 } 00171 else 00172 { 00173 Motor->INTCounter = 0; 00174 Motor->Status = MotorStatus_Running; 00175 } 00176 00177 //限幅 00178 if(Motor->PIDControl > PulseFreqLimit) 00179 Motor->PIDControl = PulseFreqLimit; 00180 00181 } 00182 00183 ////////////////////////////////////////////////////////////////////////////////// 00184 //功能：求绝对值 00185 //参数：num 带求绝对值的函数 00186 //返回值：num的绝对值 00187 ////////////////////////////////////////////////////////////////////////////////// 00188 int Math_abs(int num) 00189 { 00190 if(num < 0) 00191 return -num; 00192 else 00193 return num; 00194 } 00195 00196 //编码器初始化 00197 void Encoder_Init(MotorType *Motor) 00198 { 00199 Motor->EncoderCs->write(1); 00200 Motor->SPI_Encoder->format(16,1); 00201 Motor->SPI_Encoder->frequency(100000); 00202 } 00203 00204 //读取编码器值 00205 unsigned int Encoder_ReadData(MotorType *Motor) 00206 { 00207 Motor->EncoderCs->write(0); 00208 int response = Motor->SPI_Encoder->write(0xFFFF); 00209 response &= 0x3fff; 00210 Motor->EncoderCs->write(1); 00211 return response; 00212 } 00213 00214 ////////////////////////////////////////////////////////////////////////////////// 00215 //功能：一阶低通滤波 00216 //参数：Value_now 本次样值 Value_pre 上次滤波输出值 00217 //返回值：本次滤波输出值 00218 ////////////////////////////////////////////////////////////////////////////////// 00219 int Fliter_1(int Value_now, int Value_pre) 00220 { 00221 return (int)(FlierCoefficient*(float)Value_now+(1-FlierCoefficient)*(float)Value_pre); 00222 } 00223 00224 ////////////////////////////////////////////////////////////////////////////////// 00225 //功能：设置步进电机脉冲信号的PWM频率 00226 // 00227 //返回值：无 00228 ////////////////////////////////////////////////////////////////////////////////// 00229 void StepMotorSetFreq(MotorType *Motor) 00230 { 00231 unsigned int Motor_ARR; 00232 //为了使频率步长1Hz平滑可调需要分段设置定时器的时钟频率，即满足(Tclk/freq-Tclk/freq)>1 00233 //并且考虑到余量与ARR寄存器只有16位，设置分段如下 00234 //8k-5k 72M 00235 //5k-1k 36M 00236 //1k-500 2M 00237 //500-100 720k 00238 //100-1 72k 00239 //Motor->PIDControl = 5000; 00240 /*if(Motor->PIDControl > PulseFreqLimit) 00241 Motor->PIDControl = PulseFreqLimit; 00242 else if((Motor->PIDControl <= PulseFreqLimit) && (Motor->PIDControl > 5000)) 00243 TIM2->PSC=0; //预分频器设置 00244 else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 1000)) 00245 TIM2->PSC=1; //预分频器设置 00246 else if((Motor->PIDControl <= 1000) && (Motor->PIDControl > 500)) 00247 TIM2->PSC=35; //预分频器设置 00248 else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 100)) 00249 TIM2->PSC=99; //预分频器设置 00250 else if((Motor->PIDControl <= 100) && (Motor->PIDControl > 0)) 00251 TIM2->PSC=999; //预分频器设置 00252 else 00253 { 00254 Motor->PIDControl = 1; 00255 TIM2->PSC=999; //预分频器设置 00256 } 00257 00258 Motor_ARR = APB2Freq / (TIM2->PSC+1) / Motor->PIDControl; 00259 TIM2->ARR = Motor_ARR; 00260 TIM2->CCR2 = Motor_ARR/2;*/ 00261 if(Motor->PIDControl > PulseFreqLimit) 00262 Motor->PIDControl = PulseFreqLimit; 00263 else if((Motor->PIDControl <= PulseFreqLimit) && (Motor->PIDControl > 5000)) 00264 TIM1->PSC=0; //预分频器设置 00265 else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 1000)) 00266 TIM1->PSC=1; //预分频器设置 00267 else if((Motor->PIDControl <= 1000) && (Motor->PIDControl > 500)) 00268 TIM1->PSC=35; //预分频器设置 00269 else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 100)) 00270 TIM1->PSC=99; //预分频器设置 00271 else if((Motor->PIDControl <= 100) && (Motor->PIDControl > 0)) 00272 TIM1->PSC=999; //预分频器设置 00273 else 00274 { 00275 Motor->PIDControl = 1; 00276 TIM1->PSC=999; //预分频器设置 00277 } 00278 00279 Motor_ARR = APB2Freq / (TIM1->PSC+1) / Motor->PIDControl; 00280 //printf("ll%dll \r\n", Motor_ARR); 00281 TIM1->ARR = Motor_ARR; 00282 TIM1->CCR1 = Motor_ARR/2; 00283 } 00284 00285 //控制接口 00286 void Communication(Serial *Uart, MotorType *Motor) 00287 { 00288 char UartTemp = 0; //串口缓存 00289 if(Uart->readable()) 00290 { 00291 00292 } 00293 }
Generated on Fri Jul 15 2022 01:12:02 by
1.7.2